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CHAPTER  I 


Introduction 


As  noted  in  many  papers(e.g.  [19])  ,  light-weight  low-inertia  links  will  be  widely 
used  in  the  next  generation  of  robot  manipulators.  But  as  the  inertia  decreases, 
some  new  problems  will  arise.  For  example,  with  a  DC  motor  direct-drive  system, 
the  effects  of  ripple  torque  becomes  significant  as  the  load  inertia  decreases.  Here, 
following  the  experiment  of  Frank[ll],  a  computer-controlled  system  with  a  light 
flexible  arm  is  studied.  After  illustrating  the  experimental  setup  of  the  whole  system 
(Chapter  2)  and  deriving  the  beam  equations  under  specific  assumptions  (chapter  3), 
certain  general  steps  for  designing  a  computer-controlled  system  are  used  throughout 
this  thesis.  They  are: 

1.  Develop  a  mathematical  model. 

2.  Design  a  suitable  controller  structure  with  some  free  parameters. 

3.  By  using  optimization  packages  with  simulators,  e.g.  CONSOLE  and 
MaryLin  ,  obtain  the  optimized  design  to  meet  the  specifications  desired. 

4.  Translate  the  continuous-time  controller  to  the  discrete-time  case. 

5.  By  using  a  hybrid  simulator,  e.g.  SIMNON  ,  simulate  the  whole  computer- 
controlled  system  and  choose  a  suitable  sampling  rate. 

6.  Implement  the  controller  in  the  real-time  system. 

With  this  sequence  of  steps  and  some  tricks  in  real-time  programming,  a 
computer-controlled  system  can  be  designed  successfully. 

In  the  following  chapters,  the  controller  for  a  flexible  arm  is  first  designed  by 
CONSOLE  with  MaryLin  in  Chapter  4  to  show  how  to  use  that  powerful  package. 
In  Chapter  5,  a  new  idea  about  integral  control  is  suggested  and  is  shown  by  a 
simple  example.  The  characteristics  of  the  actuator  is  discussed  in  Chapter  6.  They 
include  friction  modeling  and  ripple  torque  in  a  DC  brush  motor  system.  One  way  to 
compensate  their  effects  has  also  been  suggested.  Chapter  7  describes  the  problems 
which  were  met  in  implementation  and  ways  to  handle  them.  After  discussing  the 
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details  of  the  experiment,  the  ideas  in  Chapter  5  and  6  are  included  in  the  design 
of  the  controller  for  the  flexible  arm  and  implemented  in  the  real-time  system.  The 
experiment  shows  that  a  combination  of  these  ideas  leads  to  notable  performance 
improvement.  The  last  chapter(8)  presents  the  conclusions  and  outlines  the  future 
work. 
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CHAPTER  II 


Experimental  Setup 


2.1  Environment 

The  system  under  consideration  is  shown  in  Fig.  2-1. 


Flexible  Beam 


Accelerometer 


IBM  PC/AT 


Figure  2-1  System  under  Consideration 


It  includes: 

1.  &  1  meter  flexible  beam. 

2.  a  DC  brush  motor 

3.  three  sensors:  position  encoder,  tachometer,  and  accelerometer. 

4.  a  Metiabyte  DDA06  A/D  L  D/A  board 
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5.  a  Metrabyte  DASH16  A/D  &  D/A  board 

6.  analog  filters  and  voltage  buffers 

7.  IBM  PC/AT 

By  way  of  A/D  and  D/A  converter  and  digital  I/O,  the  software  on  IBM 
PC/AT  can  then  control  the  system  according  to  some  specific  control  laws.  The 
details  about  the  implementation  will  be  discussed  in  Chapter  7.  In  the  next  section, 
the  computer-to-motor  model  (without  flexible  beam)  will  be  established. 

2.2  Computer-to-Motor  Model 


The  system  block  diagram  from  computer  output  to  motor  position  may  be 
modeled  as  in  Fig.  2-2. 


Fig.  2-2  Block  Diagram  of  the  Computer-to- Motor  System 


with  the  blocks: 

ZoH  :  Zero-Order  Hold 
K  :  Power  Amplifier  Gain 
R »  :  Motor  Armature  Resistance 

JRTa  :  Motor  Back  EMF  Constant 
Kt  :  Motor  Torque  Constant 
J  :  Inertia 
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By  the  input  u[kT]  from  IBM  PC/AT,  the  motor  can  be  controlled  in  some  specific 
way.  The  state  equation  of  the  whole  system  can  be  represented  as 


Ju{t)  =  T(t ) 

= 

KtKh  . .  KKt  ,  s 

=  (2.1) 

*(<)="(«)  (2.2) 

In  order  to  get  the  actual  values  of  the  motor  parameters,  it  is  necessary  to  set 
up  an  experiment  for  identifying  them.  Our  approach  of  determining  the  motor  back 
emf  constant  (Kb)  is  to  use  another  drive  (e.g.  a  tachometer)  to  drive  the  motor 
and  measuring  the  voltage  across  the  motor  armature  (i.e.  let  the  motor  be  run  as 
a  generator.)  Then  Kb  is  the  ratio  of  the  voltage  and  the  angular  velocity.  The 
angular  velocity  may  be  measured  by  the  position  encoder  and  the  time  counter  on 
the  DASH16  board. 

After  Kb  has  been  found,  the  motor  torque  constant  Kt  can  be  calculated  by 
the  formula,  as  discussed  in  [8],  corresponding  to  the  specific  unit, 

Kt  (with  the  unit  of  lb  —  in /amp)  =  8.844  Kb  (with  the  unit  of  Volt/rad/aee) 

From  the  experiment,  these  parameters  are  (nominal  value),  respectively, 

K  =  6.02 

Kb  =  2.443  (Volt/rad/sec) 

Kt  =  21.62  (lb-in/amp) 

Ra  =  33.6  (0) 

J  :  depends  on  the  load  of  the  motor 

This  model  will  be  used  in  Chapter  5  and  6  to  demonstrate  the  usefulness  of 
some  compensators. 
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CHAPTER  III 


Mathematical  Modeling 

In  this  chapter,  the  mathematical  modeling  of  the  flexible  arm  will  be  dis¬ 
cussed.  After  introducing  some  basic  principles  in  the  mechanics  and  the  calculus  of 
variations,  three  different  beam  equations  are  derived. 

S.l  Basic  Principles 

First  of  all,  Hamilton’s  principle  is  introduced. 

Hamilton ’s  Principle 

Assume  all  forces  (besides  the  constraint  forces)  are  derivable  from  a  generalized 
scalar  potential  (may  be  a  function  of  coordinates,  velocities,  and  time),  i.e.  the 
system  is  monogenic.  Then,  the  motion  of  the  system  from  time  ti  to  t2  is  such 
that  the  line  integral 

1  =  [**  Ldt, 

Jti 

where  L  =  T  —  V  where  L  is  the  Lagrangian,  T  is  the  kinetic  energy,  and  V  is  the 
potential  energy,  has  a  stationary  value  for  the  correct  path  of  the  motion,  i.e.  the 
variation  of  I 

[** 

61  —  6  I  L(9i » •  •  •  *  9n>  9i i  •  •  •  i  t)  dt  =  0.  (3.1) 

Jti 


Here,  "stationary”  means  that  the  integral  1  has  the  same  value  (to  within  the 
first-order  infinitesimal)  as  that  integral  taken  along  all  neighboring  paths. 

It  can  be  shown  [14]  that  Lagrange’s  Equations 

df_  _  d_df_ 
dqi  dt  dqi 


=  0  i  =  l,...,n 


for  mechanics  of  systems  of  particles  imply  Hamilton’s  principle,  and,  conversely, 
under  the  assumption  that  the  system  constraint  is  holonomic  (e.g.  rigid  body), 
Hamilton’s  principle  also  implies  the  Lagrange’s  equation. 
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Next,  some  basic  facts  in  the  calculus  of  variations  are  introduced.  The  proofs 
of  these  lemmas  can  be  found  in  [15]. 

Lemma.  3.1 

If  *2  (>  *1)  are  constants  and  G(x)  is  continuous  for  *i  <  *  <  Xj,  and  if 

r*» 

I  rj(x)G(x)  dx  —  0 

J  *i 

for  every  choice  of  the  continuously  differentiable  function  tj(x)  for  which  rj(xi)  = 
f/(i2)  =  0,  then 

G(x)  =  0,  Vxi  <  x  <  xj. 


The  above  lemma  can  be  extended  to  the  case  of  double  integral. 
Lemma  3.2 

If  D  is  a  domain  of  the  x-y  plane  and 

V  (*>  y)G(xt  y)  dxdy  —  0 

for  every  continuously  differentiable  function  rj  such  that 

t)  (x,j/)  =  0  V(x,y)  €  dD  (boundary  of  D) 


then 

G(x,y)=0,  V(s,y )  e  D. 


I 

Using  the  above  lemmas,  the  condition  for  the  extremum  of  some  integrals  can  be 
derived.  For  example,  the  Euler-Lagrange  Differential  Equation  can  be  derived  by 
explicitly  computing  the  variation  61  and  invoking  lemmas  3.1  and  3.2. 

For  the  case  of  one  independent  parameter  with  one  variable,  the  necessary 
condition  for  the  extremum  of  /,  where 


is 


a/  _  d_dj_ 

By  dt  By' 


(3.2) 
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If  there  axe  two  independent  parameters,  the  necessary  condition  for  the  extremum 
of  I  y  with  now 

/  =  JJD  /(*.<.  V,  Vx ,  y,t)  dx  dt, 

where  y,m  =  §*  and  y,t  =  §?  (this  convention  will  be  used  in  the  subsequent  sections, 
the  comma  is  reserved  for  the  case  where  there  is  an  index  with  a  variable),  is 


df  d  df  a  df  _Q 

dy  dx  dytX  dt  dy,t 

In  general,  for  the  n—  fold  integral 

J  =  J  ...J  /(xi,...  ,zn,tu,tu, ty,x„)  dxi 

the  necessary  condition  is 

df  d  df _ d  df  =  o 


(3.3) 


(3.4) 


dw  dx i  dwtSl  dxn  dw,%n 

For  the  case  of  multiple  variables  with  one  independent  parameter,  the  neces¬ 
sary  condition  for  the  extremum  of  the  integral 


r*a 

I  —  I  f[ty  X\ , .  .  .  ,  Xni  *1*  •  •  •  *  *n)  ^ 

Jti 


IS 


(3.5) 


^ -A|/=0,  for  *  —  l,...,n. 
dn  dt  dxi 

Similar  condition  can  be  derived  for  the  case  of  several  independent  parameters. 

Based  on  these  observations,  the  equations  of  a  beam  can  be  derived  as  in  the 
following  sections. 


3.2  Euler-Berztoulli  Beam  Equation 

Several  beam  equations  will  be  discussed  in  the  following  sections  based 
on  different  assumptions.  First,  we  consider  the  so-called  Euler-Bernoulli  Beam 
Equation.  Since  it  introduces  a  way  to  derive  the  motion  equation,  as  will  be  used 
in  the  next  two  sections,  the  details  are  included  here  as  a  comparison. 

The  beam  can  be  modeled  as  in  Fig.  3-1. 
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Figure  3-1  Approximate  Beam  Model 


where  w  =  w(x,t)  is  the  deflection  from  the  nominal  curve  and  l  is  the  length  of  the 
beam.  This  is  essentially  the  clamped-free  case  with  the  boundary  condition 


u>(0,t)  =  0 

S(°.')  =  °- 
0('.‘)  =  o- 


For  this  case,  the  potential  energy  and  kinetic  energy  can  be  found  by 


where  E  is  Young’s  modulus,  I  is  the  inertia  of  the  cross  section.  Forming  L  =  T—V , 
and  applying  Hamilton’s  principle,  the  integral 


—  EIwtXX7)  dxdt 


should  have  a  stationary  point.  Let 


/(«>>«;,„)  =  \  (pw7  -  EIwtXX7)  , 


since  /  depends  explicitly  on  tvtXX,  the  previous  necessary  conditions  derived  cannot 
be  applied.  Consider 


1  =  JJ  f(w,wiXX)dxdt, 


let  W  (x,  t)  =  w  (x,  t)  4-  tr)  (x,  t) ,  where 


y(x,t)  =  0,  V(x,t)  e  dD  (boundary  of  2?), 
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We  denote 


which  implies 


/(<)  =  //  f(W,W,ss)dxdt , 


di{€)  rr  \df(w,wtXX)dw  df{w%w,mm)aw, 
dt  J  J  d  dW  dt 


dW. 


.** 


dt 


dx  dt.  (3.6) 


Since 


we  have 


W  —  t b(x,t)  +  erj  (x,  t) 

W.zz  =  w,xz{x,t)  +  a ?,**(x,t), 


dxdt. 


m  ff  \8f  at  ] 

*  JJ  d  [awv+  8w,„v’“, 

In  order  to  have  the  variation  be  zero,  i.e.  =  0,  we  need 

df  ±  ,  8f 


dl(e) .  ff  \df  df  1 

0  -  ~sr'--0  -JJ  D 1^” + 


dxdt 


(3.7) 


= II M* ixdt + SJ  DMzv-ixit- 

By  the  Green’s  Formula, 

SS AGrx  +  F%)  ixiv  =  -JJ J  M  +  iziv+f^nlGiv-rdz). 

Let  G  =  §£,  F  =  0,  we  have 

JIM*** = ~U  AMxit+ S,DMix-  <3-8) 

Since  q{x,t)  =  0  on  dD,  which  implies  the  second  term  on  the  left  hand  side  of 
Eq.  3.2  is  zero,  we  get 

fJM%izdt=-Jh’,TM‘ix'i‘-  <3-9> 

By  another  form  of  Green’s  Formula 

SS  Da^ixiy = U/i£ixiy + L 
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Let  G  =  ,  and  with  the  boundary  conditions  for  r)(x,t),  we  get 


a/ 


,zx  dx2  JJ  d'  dx2  dw. 

By  plugging  Eq.  3.3  and  Eq.  3.4  in  Eq.  3.1,  we  derive 

a 2  a/ 


dxdt. 


°=~IJ  n”TtU;dxdt  +  JJ  D” 

-JJ. 


dx 2  dw 


■dxdt 


a 2  a/  a  af.  ,  J 

~^^r\dxdt. 


dx2  dwtX x  at  dw 


By  the  basic  lemma  3-2,  we  should  have 

a 2  a/  _  a_af_ 

dx2  dwtx*  dt  dw 

df 


0. 


Since  we  already  have 


dw 


=  —El  w. 


»** 

d 1 

dw 


=  pw> 


by  plugging  the  above  equations  in  Eq.  3.12,  the  equation 

—  rd4w  d2  w 
El  —  —  +  p  =  0 


dx * 


at 2 


has  been  derived. 


(3.10) 


(3.11) 


(3.12) 


(3.13) 


3.3  Discretized  Beam  Equation 

Next,  we  are  going  to  derive  another  equation  also  illustrating  the  motion  of  the 
beam.  Now  the  model  of  beam  is  shown  in  Fig.  3.2.  The  beam  is  mounted  (hinged) 
on  a  rotational  hub. 
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Now  the  large  scale  motion  of  beam  is  taken  into  account,  where  9  is  the  angle  of 
the  shadow  beam,  y  is  the  bending  deflection  from  the  shadow  beam,  and  x  is  the 
distance  from  the  root  to  a  certain  point.  With  the  parameters  as  defined  before, 
the  kinetic  energy  and  potential  energy  can  be  now  computed  as 

T=\J'P(x9  +  y)*dx 

•  =\J  Px*dx92  +  J  pxydx  9+^  J  py2dx>  (3.14) 

i  rl 

V=-  Ely^dx  +  T^ 

*  Jo 

Q  2 

where  y,CK  =  .  Since  now  the  beam  is  rotating  through  an  angle  9,  there  should 

be  some  external  torque,  i.e.  !T«xt  and  assume  that  £!T«xt  =  —rS9. 

Here  the  discretization  method  will  be  used  in  representing  y,  i.e.  assume 

n 

y  =  ?<(*)&(*)• 

*=i 

It  is  essentially  a  finite  approximation  of  n  modes.  Thus, 


V  =  53 &(*)&(*) 


»=1 

where  &,««  =  .  By  plugging  y  and  y,«,  into  the  equation  (3.14), 

T~\So  px*dx  *2  +  lo  PX  ?  dxP+\J0p  f  !C  &(*)&(*)! 

'  *  J  \  •  / 

V  =  dx  +  T%xt. 

Letting  L  =  T  —  V ,  by  Hamilton’s  principle, 


dx 


(3.15). 


6  f  *  Ldt  =  0. 
Jt  i 
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Since  L  =  L(t,0,O}qi,. . .  ,qn,qit. . .  ,qn) ,  this  is  case  of  one  parameter,  multiple 
variables  case.  Thus  the  formula  (3.5)  can  be  used,  i.e. 


'  dL  _  =  n 

dO  dtdO~° 
dL  d  dL  n  ,  . 

{  dqi  dtdqi~°'  for*  “ 


Since 


dL 

do 

dL 

dO 


—  T 


(3.16) 


r  rl  M 

r  =  /  px2dxO  +  I  px}  qj<f>jdx 
9  Jo  Jo  t 

~q^~.  =  ~  J0  "^OL^®*^****)  ^j.xxdx 

dL  rl  .  rl  /V-^ 

—  =  px<f>jdxO  +  p{2_^0i<f>i)<Pjdx, 

by  plugging  them  in  Eq.  (3.16),  we  get 

L  ft'ixi+'£  S,  pxfcdxqi  =  r 

T.  EI4>i,*,4j*,ixqi  +  jf  p4i$jdxqi  \  —  -  J  pxfadzS,  j  = 

(3.17) 

If  Eq.  3.17  is  put  into  the  state-space  form,  e.g.  mq  +  kq  =  6r,  then  it  is  exactly  the 
same  equation  as  derived  in  [16]  . 


3.4  Geometrically  Exact  Beam  Equation 

Another  model,  the  so-called  geometrically  exact  model  for  the  plane  motion  of 
the  beam  will  be  derived  as  follows.  It  takes  account  of  the  shear  deformation  as  well 
as  the  flexure  deformation  in  the  motion.  Now  the  motion  of  the  beam  can  be  drawn 
as  in  Fig.  3-3,  where  {ej,/  =  1,2,3}  is  the  reference  coordinates  with  es  =  c\  x  e2, 
{</,  I  =  1, 2, 3}  is  the  deformed  coordinates  with  t3  =  x  t2 ,  X  is  a  point  on  the 
beam  in  the  reference  configuration  and  x  is  the  point  where  the  X  has  been  moved 
to. 
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Figure  3-3  Exact  Beam  Model 


X  and  x  can  be  represented  in  the  form  as,  with  different  coordinates, 


X=Xxex  +  X2e2 
x  =  +  X2t2 

=  [xx  +  «i  [Xu  0]  *1  +  [*»  fa,  0]  h  +  x2i2. 


(3.18) 


where  ^o(Xi,t)  is  the  line  of  centroid  of  the  beam,  and  the  axis  transformation  is 

tx  =  cos  Bex  +  sin  Be 2 

t2  =  -  sin  Bex  +  cos  Be 2  (3.19) 

(  *3  =  Cs- 


and 


dB  t 

dXi  ~  dXi' 
8B  : 

~axxtu 


dij_ 
<i 

at2 

axi 


aix  .. 

ar-*” 


at2 

at 


=  -BtX 


So  the  derivative  of  £  can  be  found  as 


£  =  4>0  +  X2i2 

=  (tii  —  BX 2  cos  B)ix  +  (uj  —  BX i  sin  B)e 2. 


and 


lill2  =  i-i 


=  (it*  +  u2)  ■+■  B2Xl  —  2BX2  (tii  cos#  +  tt2  sin  I 
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The  kinetic  energy  of  the  system  can  be  then  computed  as 

T=\f\Wix1 

=  i  /  [(uj  +  *1)  +  t'X I  -  29Xj(ti,  cos  l  +  i2  sin  «)]  dX2 iX, 

=  |  J‘[( f_k  pdX2)(il  +«1)  +  ( „XUX,)P 

—  2(J  ^  pX2dX2)$(ui  cos 0  +  u,  sin  0)  dX2 
Assume  that  the  beam  is  symmetric,  the  integral  pX2dX2  =  0.  Let 


_  A 

Ae  =  /  pdX2,  and 

J=£ 

Ip  =  p*?dX2. 


Then,  Eq.  3.13  is  reduced  to 

Next,  the  potential  energy  will  be  constructed.  First,  the  strain  field  is  introduced. 
When  the  beam  undergoes  large  strains,  the  strain  field  can  be  defined  as 

A 

*  =  _  f 

1  ax2  1  (3.20) 

=  (1  +  «i,*  —  cos  9)e  i  +  (ti2,*  —  sin  0)e2. 

(ux,*  really  means  that  ,  for  the  reason  of  simplicity,  the  simpler  notation  will  be 
used  here  as  well  as  in  the  subsequent  discussions.)  Then,  relative  to  the  coordinate 
system  {£/},  from  Eq.  3.16,  this  field  can  be  written  as 

*1  =  (1  +  «x,*  -  cosff)(cos0ti  -  sin0*2)  +  (u2(I  -  sin  0)(sin0£x  +  cost2) 

=  (cos  0  +  ux,*  cos  9  +  tt2t,  sin  9  -  l)ti  +  (-  sin  9  -  u1<x  sin  9  +  «2(t  cos  0)t2. 
Now  the  axial  strain  Tx  and  shearing  strain  T2  are  defined  as,  respectively, 


Tx  =  cos  0  +  ux|Se  cos  0  +  tt2>*  sin  9  —  1 
r2  =  -sin*-  tti, *  sin  9  +  u2,z  cos  9. 


(3.21) 
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Let  EA  be  the  axiaJ  stiffness,  GA ,  be  the  shear  stiffness,  and  El  be  the  GexuraJ 
stiffness,  then  the  potential  energy  can  be  written  a a 

V  =  \  [\eAT\  +  GA.Tl  +  EIB,l)  dXx  -  n.xt  -  T.xt{t)9{0,t)  (3.22) 

2  Jo 

where  IT.xt  is  the  potential  energy  of  the  external  loading  acting  on  the  beam  and 
r#xt(i)«3  is  the  applied  torque  at  the  axis  of  rotation  £3  of  the  beam.  Now  we  assume 
that  there  is  no  external  loading,  i.e.  n.xt  =  0.  Then,  the  lagrangian  L  can  be  found 


L  =  T  —  V 


-n 


+  if)  +  \ 1,6*  -  |(EATf  +  GA.T\  +  Elf,  f )  + 


dX  1. 
(3.23) 


Hamilton’s  principle  requires  that  f**  L  dt  to  be  stationary.  Let 

f{Xi,t,UitU2,9,ultu2,9tuit„uliS,etS) 

=  jA,(if  +  if)  +  -  i(EATl  +  GA.Tl  +  EIf.l)  +  ?ft. 

Then,  it  is  the  case  of  three  dependent  variables  with  two  independent  parameters. 
By  the  formula  from  the  calculus  of  variations,  analogous  to  Eq.  3.3,  the  equations 
are 


df__ 

a  df 

d  af  -0 

dui 

dX  1  dui,* 

at  du\ 

df 

a  aj 

a  ar-0 

dll2 

dXi  du2,x 

at  dv.2 

df 

a  aj 

aaf  _0 

dO 

ax  1  ae,s 

dt  d9 

(3.24) 


The  only  thing  left  is  to  find  the  partial  differentiations  and  then  plug  into  Eq.  3.24. 
We  have 


=  0 


=  —EAT  1  cos  9  +  GA,T2  sin  9  =  n\ 


1L 

dui 

df 

dui)g 

V  dux  ~  ApUl' 
thus,  by  the  first  equation  in  Eq.  3.24, 

Apii  +  »»i,,  =  0. 


(3.25) 
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From 


we  get 


df_ 

du2 

df 

du2,m 


=  0 


=  —EAT i  sin  9  -  GA,T2  cos  0  =  n2 


Hi =AA 


Afiu2  ■+■  **2,*  =  0. 


(3.26) 


From 


[  —  =  EAT i  sin  9  +  GA,T2  cos 9  4-  «i ,x{EATi  sin  9  +  GA,Ti  cosfl) 

o9 

T„t 

+  U2,x{-EAT  1  cos  9  +  GA.Ti  sin  0)  H - j— 


1L 

ae 


T,xt 

=  — n2  —  ui,je»12  +  H - — 

=  -E/0.* 


.* 


£/ -  />, 
i  a«  ' 


the  third  equation  can  be  obtained  as 


Ip9  +  (1  +  «xlS)n2  EIO'Zx  —  ^ 


ext 


(3.27) 


From  Eq.  3.25,  3.26,  3.27,  the  governing  equation  can  be  then  written  in  the  matrix 
form, 


1 

o 

o 

<*. 

wi,* 

0 

0  Ap  0 

u2 

+ 

n2,x 

= 

0 

0  0  Ip 

9 

_  (1  +  t*l,*)«2  —  «2,*nl  —  EI9,zz  _ 

LvJ 

(3.28) 


This  is  the  geometrically  exact  beam  equation  for  the  planer  motion  of  a  flexible  arm. 
If  the  assumption  of  n.xt  =  0  in  Eq.  3.22  is  changed  to 


Ilext 


•  9e2  +  fl  •  ^o)  dX\ 


with 


1h  =  fh[Xi,t)e2 
h  =  fiit-X’ijt)*!  +  ft2(-Xl»0*2> 


and  T.x t  =  0  is  assumed,  then,  by  the  same  derivation  method,  the  beam  equation 
is  exactly  the  same  with  in  [17]  . 

Although  this  beam  equation  is  quite  realistic,  for  the  reason  that  it  is  too 
complicated,  it  is  not  used  in  the  following  chapters.  The  beam-hub  model  used  later 
is  essentially  borrowed  from  [ll]  ,  and  is  experimentally  determined. 
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CHAPTER  IV 


CONSOLE  Design 


4.1  Introduction 

Optimization-based  design  is  an  effective  means  to  solve  design  problems  with 
constraints.  Although  there  is  a  number  of  sophisticated  methods  in  the  field  of 
optimal  control  to  fulfill  this  purpose,  few  among  them  take  the  advantages  of 
recent  powerful  optimization  algorithms  and  numerical  CAD  package  in  doing  the 
design.  DELIGHT.Marylin[l3]  is  such  an  optimization  tool  for  designing  linear  time 
invariant  control  systems.  It  is  however  cumbersome  to  use  and  to  maintain.  Another 
optimization  package  called  CONSOLE  has  been  recently  developed.  It  mainly 
consists  of  two  programs:  CONVERT  and  SOLVE,  and  can  be  connected  with  any 
simulators  designed  by  the  user,  e.g.  MaryLtn  (a  simulator  for  linear  time-invariant 
control  systems.)  With  the  help  of  CONSOLE  ,  the  designer  can  automatically 
adjust  certain  controller  parameters  to  meet  some  specific  requirements  without 
getting  involved  in  the  details  of  optimal  control  theory. 

The  details  of  how  to  use  CONSOLE  and  how  CONSOLE  works  can  be 
found  in  the  CONSOLE  manual  [10].  The  next  section  will  give  an  example  of  how 
to  design  a  controller  for  the  flexible  arm  by  using  CONSOLE  with  the  simulator 
MaryLtn  . 


4.2  Designing  a  Controller  for  the  Flexible  Arm 

The  flexible  arm  described  in  Chapter  2  has  been  identified  as  a  linear  time- 
invariant  system[ll]  and  its  transfer  function  from  hub  position  to  tip  acceleration 
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is  modeled  as 


*(•) 


_ °-125«[(2^  x  9,5)2  +  2ir  x  9.5  +  ^ _ 

.  «»  2(°-n).  ,, _ ^ _  2{0.02),  .  ,l 

l2irxl.l6  '‘(2ITX  9)1  2*  x  9  1 1  (2*  x  20.6)1  2*  x  20.6  J 


In  order  to  reach  the  states  by  the  three  sensors,  position  encoder,  tachometer, 
accelerometer,  the  above  transfer  function  can  be  decomposed  as 


H(s)  = 


2.454 


0.134[ 


(19tt): 


2.1s 

+  T7T-  +  !] 
19tr  J 


0.22s 

18jt 


+  »][ 


(41.2ir): 


0.04s 


0.383s2 


acceleration 


motor 


beam 


The  state  space  representation  of  the  system  is  thus  the  following: 


■  -17.618 

1 

0 

0 

0 

0  ■ 

•  0  ■ 

i2 

-20015.2 

0 

1 

0 

15035.99 

0 

0 

is 

*4 

— 

-224975.8 

-53572128.7 

0 

0 

0 

0 

1 

0 

1795004.1 

53572128.7 

0 

0 

x  + 

0 

0 

is 

0 

0 

0 

0 

0 

1 

0 

.is. 

0 

0 

0 

0 

0 

-7.29. 

.17.96. 

M  H 

ss 

1  0 
-999.6  -0.89470.0507 

0  0  ( 
0  762.73 

3  O' 

3  J* 

where  y\  is  the  tip  position,  y 2  is  the  tip  acceleration. 

The  state  feedback  controller  is  chosen  to  attain  the  requirements,  i.e.  u  = 
—Kx  +  v,  where  v  is  the  reference  input.  Thus,  there  are  six  design  parameters  in 
this  problem.  Listing  4-1  shows  the  problem  description  file  which  will  be  read  by 
CONVERT. 


Listing  4-1  Problem  Description  File 


PI  -  S . 141502053 
El  -  1/(9*2*PI) 

E2  -  1/(20. fl*2*PI) 

ES  «  1/(9.6*2*PX) 

daalgn.paraaatar  kl  init»8 .23403O9E8a«OO  vari“1.43a0 
daalgn.paramatar  k2  init*-l.B8346004a-01  vari»9a-2 
daalgn.paramatar  k3  lnlt*-1.679000251a-03  Tari“8.4a*4 
daalgn_paramatar  k4  init«2.3870BB66Ba-5  Tari"1.7a-6 
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dealgn.parametar  kS  init«4. 089930087*1  vari«4.34*l 
daaign.paranatar  kd  init«6 . 863*-l  vari"2a-l 


functional.obj active  "over-ahoot" 
for  t  from  0  to  3  by  .01 
minimize  { 

doubla  YtrO ; 
return  Ytr(Ml",t); 

> 

good.curva  ■  { 

if(  t  <•  1.  )  ratura  1.08; 
ala*  ratura  1.02; 

> 

bad.curva  ■  { 

if  (  t  <*  1.  )  ratura  1.15; 
ala*  ratura  1.0E; 

> 

functional.obj active  "Battling" 
for  t  from  0.0  to  3  by  .01 
maximize  { 

doubla  Ytr(); 
ratura  Ytr("l".t); 

> 

good.curva  «  { 

lf(  t  <■  0.8  )  ratura  0.9; 
ala*  ratura  0.99; 

> 

bad.curva  ■  < 

if(  t  <«  0.8  )  ratura  0.86; 
ala*  ratura  0.95; 

> 

objective  "ateadyatato" 
minimize  { 

doubla  Ytr() ; 

ratura  faba(  Ytr("l",5.0)-1  ); 

> 

good.valu*  »  0 
bad.valu*  ■  l*-8 

functlonal.coaatraiat  "max-accal"  bard 
for  t  from  0  to  3  by  .01 
i 

double  YtrO; 
ratura  Ytr("2",t); 

> 

<■ 

good.curva  »  {  ratura  10;  > 
bad.curva  ■  <  ratura  11;  > 
fuactloaal.conatraiat  "max-accal"  hard 
for  t  from  0  to  3  by  .01 
i 

double  YtrO; 
ratura  Ytr("2",t); 

> 

»• 

good.curva  ■  <  ratura  -10;  > 
bad.curva  ■  <  ratura  *11;  > 
functlonal.coaatraiat  "control"  hard 
for  t  from  0  to  3  by  .01 
< 


21 


Import  kl  k2  k3  k4  kB  kA 
doubl*  YtrO; 

return  f aba  (  1- <kl*Ytr ("3" . t)  *k2*Ytr  ("4" . t)  *k3*Ytr ("6" , t)  *k4*Ytr (•'«" , t) 
♦k6»Ytr ("7" , t) ♦k6»Ytr ("8” , t)  )); 

>  <• 

gOOd.CUTYS  *  < 

return  4; 

> 

btd.curva  ■  { 
return  B; 

> 

The  desired  position  profile  is  shown  as  in  Fig.  4-1,  and  is  set  by  the  first  two 
objectives. 


Figure  4-1  Desired  Position  Profile 


Since  the  control  signal  (voltage  across  the  motor)  can  not  be  too  large,  the 
constraint  for  it  is  set  by  the  functionaljconstraint  named  “control”.  The  constraint 
for  the  tip  acceleration  is  set  by  the  other  two  functionaljconstraints  to  limit  the  tip 
acceleration  in  certain  range. 

Listing  4-2  gives  the  system  description  which  will  be  read  by  MaryLin  and 
linked  with  SOLVE.  It  is  a  straightforward  translation  from  the  state  representation 
of  the  system  as  in  Eq.  4.1.  The  feedback  gains  were  included  in  the  A  matrix.  The 
reference  input  now  is  set  by  Ut. 

Listing  4-2  System  Description  File 

PI  -  3 . 1415026 

G  -  0.12S 

R1  -  Q*2*PI 

R2  -  20 .0*2*PI 

R3  -  0.6*2*PI 

R4  «  G  /  (0.383  *  2.124) 
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A1  -  0.04*R2*0.22*R1 

A2  -  R1*R1*0.0088*R2*R1*R2*R2 

AS  -  0.22*R2*R2*R1*0.04*R2*R1*R1 

A4  -  R2*R2*R1*R1 

B2  -  R1*R2*R1*R2/(R3*R3) 

B3  -  2*B2*R3 

84  -  B2*R3*R3 

Cl  -  (A1*A1-A2) *0 . 383 *R4 

C2  >  -A1*0 . 383*R4 

C3  -  0 . 383*R4 

C4  -  B2*0.383*R4 

bO  -  2*PI*1. 16*2. 124 


ayatam.alza  Ninputa« 

1  Netataa 

>6  KoutputaaR 

raadmatrlz  A 
-A1  1 

0 

0 

0 

0 

-A2 

0 

1 

0 

82 

0 

-A3 

0 

0 

1 

B3 

0 

-A4 

0 

0 

0 

B4 

0 

0 

0 

0 

0 

0 

1 

-b6*kl 

-bfl*k2 

-b6*k3 

-b6*k4 

-b6*kS 

-7.20-b6*k6 

raadmatrlz  B 
0 
0 
0 
0 
0 

be 

raadmatrlz  C 

1  0  0  0  0  0 

Cl  C2  C3  0  C4  0 

1  0  0  0  0  0 

0  1  0  0  0  0 

0  0  1  0  0  0 

0  0  0  1  0  0 

0  0  0  0  1  0 

0  0  0  0  0  1 

raadmatrlz  Ut 
1 


The  CONSOLE  tandem  works  as  follows:  CONVERT  reads  the  problem 
description  file  and  SOLVE  then  calls  the  simulator  MaryLin  which  reads  the 
system  description  file  to  optimize  the  system  performance  subject  to  the  changing 
of  design  parameters. 

The  initial  guess  of  the  design  parameters  is  chosen  in  the  same  way  as  in  [ll]. 
By  interacting  with  SOLVE  for  several  iterations,  the  final  values  of  the  design 
parameters  were  obtained  as  given  in  Listing  4-3.  The  pcomb  (performance  comb) 
output  is  also  given  there.  It  shows  how  close  each  of  the  constraints  or  objectives  is 
to  their  corresponding  good  and  bad  value. 
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Listing  4-3  Designed  Value  of  Design  Parameters  and  Peomb  Output 


Nana 

Valoa 

Variation  art  0  Prav  Itar*30 

kl 

8.23404a+00 

1.4a  *00 

k2 

-1.68345a-01 

0.0a -02 

kS 

-1.570QQa-03 

8.4a-04 

k4 

2 . 38706a-05 

1.7a -05 

k6 

4.08Q04a+01 

4.3a«01 

M 

S.  85300a -01 

2.0a -01 

Peomb 

(Itar*  30)  (Phaaa 

2)  (apa-  1.000a+00)  (NAX.COST.SOFT-  0.0341) 

BAD 

1 . 00a-04 
1.10* ♦00 
0 . 60a-01 
l.lOaaOl 
1 . 10a*01 
6.00a»00 


SPECIFICATION  PRESENT  GOOD  C  B 

01  ataadyatat  6.72a-06  O.OOa+OO  | 

F01  ovar-ahoot  l.OQa+OO  1.01a+00  ■■■■■■■■■■■■■■■■»■■»* | 
F02  Battling  0.01a-01  0.00a-01 

PCI  aax-accal  2.71a»00  l.OOa^Ol  <—  I  | 

FC2  nax-accal  -8.71a-01  -l.OOa+Ol  < . - . 

FC3  control  1.03a^00  4.00a+00  <--  |  | 


The  plots  of  tip  position  and  control  signal  are  shown  in  Fig.  4-2  and  Fig.  4- 
3,  respectively.  With  this  design,  the  control  law  can  be  then  implemented  in  the 
real-time  system.  The  details  of  the  implementation  will  be  described  in  Chapter  7. 


Functional  Objactira  oTar-ahoot 


X10° 


Figure  4-2  Plot  of  Tip  Position 
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Functional  Constraint  control 


0  1  2  3 

Figure  4-3  Plot  of  Control  Signal 
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CHAPTER  V 


Integral  Control 


5.1  Introduction 

In  order  to  compensate  steady  state  errors,  we  use  a  scheme  based  on  integral 
control.  The  system  is  augmented  with  an  artificial  state  which  will  be  used  in  the 
integral  feedback.  This  chapter  will  investigate  some  properties  of  the  augmented 
system  and  how  to  use  this  idea  in  the  control  of  a  motor. 


5.2  The  Augmented  System  (SISO  Case) 

The  general  SISO  (single-input-single-output)  system  can  be  modeled  as 


where 


x  =  Ax  +  bu 
y  =  cx  +  du 


A  :  n  x  n,  b  :  n  x  1,  c  :  1  X  »,  d:lxl. 


(5.1) 


The  idea  is  to  use  the  integral  of  the  output  as  part  of  the  feedback.  The  control  law 
may  be  written  as 

ti  =  —Kx  —  K\  Jy  +  v  (5.2) 

where  v  is  the  reference  input. 

The  integral  term  is  accounted  for  by  a  new  state  z.  Append  to  Eq.  5.1  the 
equation 

z  =  pz  +  my.  (5.3) 


Then  for  p  =  0,  the  new  state  t  is  exactly  m/y.  Therefore,  the  control  law  in 
Eq.  5.2  is  exactly 

u=—  Kx-—  z  +  v  (5.4) 
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The  integral  control  problem  is  thus  transformed  into  a  state-feedback  control 
problem  with  an  augmented  state  z. 

The  new  system  may  be  written  as 

x  —  Ax  +  bu 
z  =  pz  +  my 
y  =  cx  +  du 

where  p,  m  are  scalars.  By  plugging  y  into  i, 

z  =  pz  +  mcx  -(-  mdu, 

and  the  augmented  system  can  be  represented  as 


Following  a  discussion  of  the  properties  of  the  augmented  system,  we  will 
present  the  controller  design  using  state  feedback  for  this  new  system. 

5.3  Some  Properties  of  The  Augmented  System  (SISO  Case) 

This  section  will  investigate  some  properties  of  the  new  system  (5.5).  Obviously, 
if  the  original  system  (5.1)  is  stable,  then  the  augmented  system  (5.5)  is  stable 
when  p  <  0.  For  the  controllability  and  observability  of  the  augmented  system,  the 
following  two  propositions  give  the  necessary  and  sufficient  condition. 

Proposition  5.1 

Assume  the  original  system  (5.1)  is  controllable,  and  let  its  transfer  function  h(s)  to 
be  "1^1  •  Then  the  augmented  system  (5.5),  with  m  ^  0,  is  controllable  if  and  only 
if  p  is  not  a  root  of  j(a) . 

Proof 

By  the  PBH  rank  test  for  controllability [9],  first  form  the  matrix 

Q  =  [sl  -  A  5] 

\bI-A  0  b 
—me  s  —  p  md 
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The  system  (5.5)  is  controllable  if  and  only  if  Q  is  of  full  rank  for  all  s. 

For  a  p,  the  last  row  is  independent  of  the  other  rows,  since  the  original 
system  is  controllable,  thus  Q  is  of  full  rank.  For  the  case  of  a  =  p, 

Q  is  of  full  rank 


<=> 

pi 

^  ^  1  is  nonsingular, 

me  md  J 

<=*• 

det 

’lit  mi]*0’ 

det(p/  -  A){mc(p/  —  A)  *6  +  md)  ^  0, 

<=> 

ma(p)h(p )  ^  0, 

mq{p)  #  0, 

p  is  not  a  root  of  g(s). 

QED  | 


As  for  the  property  of  observability  of  the  augmented  system,  if  the  matrix 

al  -  A  O' 

—me  a  —  p 
e  0 

is  formed,  then  for  a  =  p,  it  will  never  be  of  full  rank.  However  if  we  revise  the 
augmented  system  to  be  observed  as 

y=[e  !][*]+<*«,  (5.6) 

then  since  the  state  z  is  artificial,  it  can  always  be  observed.  It  is  easy  to  get  the  new 
output  from  the  old  one.  With  this  new  revised  system,  the  following  proposition 
gives  the  condition  for  preserving  observability. 


_  si  -  A  _ 
~  [  1  ~ 


Proposition  5.2 

Assume  the  original  system  (5.1)  is  observable.  The  revised  augmented  system  with 
output  (5.6)  is  observable  if  and  only  if  p  —  m  is  not  an  eigenvalue  of  matrix  A. 


Proof 


As  before,  by  the  PBH  rank  test  for  the  observability  with 

R  = 


al  —  A  0 
—me  a  —  p 
e  1 
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For  8  =  p, 


R  = 


el  —  A  0 
—me  0 
e  1 


Obviously,  R  is  of  full  rank  when  the  original  system  1b  observable.  For  8  ^  p,  by 
the  elementary  row  operations,  R  is  equivalent  to 


R~ 


Thus,  for  R  to  be  of  full  rank, 
el -A 

(-m  +  p  -  s)ej 


al  —  A  0 
(— m  +  p  —  a)c  0 
e  1 


must  be  of  full  rank  for  all  s  ^  p 


For  — m  +  p  —  8  /  0,  it  is  directly  implied.  For  -m  +  p  —  8  =  0,  we  should  consider 
the  matrix 

^  ^  j  to  be  of  full  rank 

<=>  [(p  —  m)I  —  A]  must  be  nonsingular, 

<=>  det  [(p  —  m)I  —  A]  ^  0, 

<=>  a(p  -  m)  /  0, 

<=>  p  —  mis  not  a  root  of  c(«). 

<=>  i.e.  p  —  m  is  not  an  eigenvalue  of  matrix  A. 

QED  | 


5.4  Designing  the  Controller  with  CONSOLE 

The  feedback  gains  KtKj,  as  well  as  the  augmented  state  parameters  m,p, 
can  be  designed  by  using  CONSOLE  .  Simply  declare  the  state  parameter  p,  m  and 
the  feedback  gain  K>  Kj  as  the  design  parameters,  with  the  simulator  Mary  Lin  . 
The  optimization  process  in  CONSOLE  is  used  to  get  a  best  design  in  some  sense. 
The  conditions  for  stability,  controllability,  observability  can  be  set  as  constraints. 
Next,  a  simple  example  will  be  given  to  illustrating  this  method.  The  application  to 
the  flexible  arm  control  will  be  described  later. 


29 


Example 

A  motor  system  can  be  modeled  as 


R.  R.Ki 

$  =  UJ. 


U) 


(5.7) 


Let  xi  =5  and  X3  =  w,  the  state-space  representation  of  the  whole  system  may  be 
written  as 


•  ^  -  ITT 

x\  0  1  x\ 

•  =  A  /Cf 

L*2  J  L°  L*2j 


o 

j*.  j 


V  =  [  1  0 


)N. 

L*2J 


By  adding  the  augmented  state  equation  z  =  — pz  +  my  (note  that  in  order  to 
preserve  the  positiveness  of  p,  a  minus  sign  included),  the  augmented  system  is 


*1 

r 

ia 

ii 

z 

L 

0 

0 

m 


1 

K_t_ 


JR.K* 

0 


0 

0 

-p 


* 

*i 

0 

x2 

+ 

KKt 

JR. 

z 

0 

u 


V  =  [  1  o  1] 


*1 

*a 

* 


The  transfer  function  of  the  original  system  is 

KKt 


JR. 


I  .  K,  , 


Thus,  by  the  Proposition  5-1  and  5-2,  — p  —  m  cannot  be  0  or  —  and  there  b 

no  constraint  for  p. 

The  controller  b  chosen  a a  U  =  -KiX\  —  Ktii  —  K$z  +  V .  By  letting  m, 
p,  K x,  JCa,  As  be  the  design  parameters,  CONSOLE  can  be  used  to  design  the 
system.  Lbting  5-  1  shows  the  input  file  to  CONSOLE  . 


Lbting  5-1  Input  File  to  CONSOLE  for  Integral  Control 

/**•  Integral  Control  Problem:  problem  description  file  *•*/ 

Kt  ■  21.62 
Kb  -  2.443 
It  •  33.6 
J  «  0.1 
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design.parametar  K1  init»8. 38833  vari«3 
dasign.paramatar  K2  init»-l .612E6a-l  varl-0.00 
dasign.paramatar  K3  init"-1.37716ai  Ttri>8 
dasign.paramatar  m  init-1 ,33830al  varies 
deaign.paramatar  p  init"2. 40271*1  mln»0.01  vari«10 
/***  Neat  tha  anglneering  specification  **•/ 
functional.objactiva  "ovar" 
for  t  from  0  to  2  by  0.005 
mlnlmiza  {  doubla  YtrO  ; 

raturn  Ytr(l.t):  > 

good.curva  ■  {  if(  t  <»  0.6  )  raturn  i.i 

alaa  raturn  1.01;  > 

bad.curva  ■  {  if(  t  <■  0.6  )  raturn  1.5; 

alaa  raturn  1.06;  > 

functlonal.objactlva  "undar" 
for  t  from  0.4  to  2  by  0.005 
mazlmlza  {  doubla  YtrO; 

raturn  Ytr(l.t) ;  > 

good.curva  »  {  if(  t  <■  0.6  )  raturn  0.00; 

alaa  raturn  0.00;  > 

bad.curva  ■  <  lf(  t  <■  0.6  )  raturn  0.86; 

alaa  raturn  0.06;  > 

constraint  "obsarv"  bard 
{  Import  m  p  Kt  Ra  Kb  J 

raturn  f aba (  (-p-m)*(-p-m)*Kt*(-p-m)/(J*Ra*Kb)  ); 

>  >"  good_valua»i 

bad.valua  "0.0001 

•*<**  Syatam  Daacrlptlon  Flla  for  lntagral  control  ttSft 

K  -  0 

Kt  -  21.62 

Kb  -  2.443 

Ra  ■  33.0 

J  -  0.1 

C  •  K  *  Kt  /  (J  •  Ra) 

aystaa.alza  Nlnputs"l  Natataa-3  Noutputa"l 
raadmatrlz  A 

0  1  0 

-G*K1  -Kt/(J*Ra*Kb)-G*K2  -G*K3 

a  0  -p 

raadmatrlz  B 
0 
C 
0 

raadmatrlz  C 
10  0 
raadmatrlz  Ut 
1 


The  constraint  “observ"  is  set  for  satisfying  the  condition  of  —  p  —  m,  i.e. 

(-P  -  m)2  +  x  (-p  -  m)  #  0. 

After  several  iterations,  CONSOLE  gets  the  designed  parameters  that  meet 
the  specifications.  The  position  output  0  plot  from  CONSOLE  is  shown  in  Fig.  5-1. 
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Functional  Objective  thotn 


Listing  5-2  shows  the  pcomb  result  and  the  final  values  of  design  parameters. 


Listing  5-2  Pcomb  and  Desired  Design  Parameters  from  CONSOLE 


Pcomb (It er"0) PRESENT  GOOD  6  B  BAD 

F01  over  1.01e*00  1.01e+00  ■»—■■■•  |  |  l.OEeeOO 

F02  under  9.99e-01  9.90e-01  9.50e-01 

Cl  obaarv  1.37e»03  l.OOe+OO  < — - -  1 . 00a-04 

<0> 

design  psrsaetar(s)  for  ltarstlon  0 


Name  Value 

Ki  8 . 38833e+00 

K2  -1 .61255e-01 

KS  -1.37716e*01 

a  1.33839e*01 

p  2.492710*01 


This  controller  will  be  implemented  in  the  real-time  system  with  the  flexible 
arm  removed.  Since  there  are  already  two  sensors:  position  encoder  and  tachometer, 
the  only  state  needed  to  be  reconstructed  is  a.  In  « -domain,  from  Eq.  5.3, 

Y{b)  (5.8) 

s+p 
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By  the  straight-forward  sampling  of  Eq.  5.8,  the  discrete-time  system  is 


where  q  is  the  forward  shift  operator  and  h  is  the  sampling  period.  Then,  with  the 
velocity  form[ 2],  the  state  z  can  be  found  by 


i|M>]  =  e-'MO'  -  l)k]  +  j(l  -  -  l)h]. 

In  order  to  have  an  idea  of  how  the  controller  works  in  the  discrete  form,  the 
simulation  package  SIMNON  is  used  to  simulate  the  whole  system:  the  continuous¬ 
time  plant  with  a  discrete-time  controller.  The  sampling  rate  of  the  system  can 
also  be  decided.  Listing  5-3  is  the  input  file  for  SIMNON  .  System  CMOTOR  is 
the  plant,  DMOTOR  is  used  to  reconstruct  the  state  z  and  compute  the  feedback, 
MOTORCON  sets  up  the  connection  between  the  plant  and  the  controller. 


Listing  5-3  Input  File  to  SIMNON  for  Integral  Control 
continuous  system  CMOTOR 

"Continuous-Time  linesr  system  for  the  motor 

INPUT  u 

OUTPUT  yl  y3 

state  xl  x3 

DER  4x1  4x3 

dxl«0*xl*l*x3*0*u 

dx3*0*xl-Kt/ (J*Re*Kb) *x3*K*Kt/ ( J*Re) *u 

y»cl*xl+c3*x3 

yl“xl 

y2»x3 

K:0 

Kt:21.62 
Kb : 3 . 443 
J:0.1 
Re: 33.0 
cl :  1 
c3:0 
END 

discrete  system  DMOTOR 
"Discrete-Time  Integrel  Feedback 
INPUT  yr  yl  y3 
OUTPUT  u 

state  1 

new  ni 
time  t 
tsemp  ts 

n»yr- (kl*yl+k3*y3»k3*i) 
ni*exp(-p*h) *i*m* (1-exp (-p*h) ) *yl/p 
ts“t+h 
kl:8. 45158 


33 


k2:-1.14096*-l 
kS:-l. 42890*1 
p: 2. 03078*1 
a:  1.37620*1 
h:0.006 
END 

connecting  ayatam  aotorcon 

"Connection  for  *lBulatlon  of  th*  aotor  *y»t*a  and  integral  controller 
tin*  t 

yr [dmotor] *1 
y 1 [dmotor] *y 1 [emotor] 
y2 [dmotor] «y2 [emotor] 
a  [emotor]  *01  [dmotor] 

END 


The  simulation  result  for  the  sampling  time  0.005  sec  is  shown  in  Fig.  5- 
2.  When  we  increase  the  sampling  time  beyond  0.5  sec,  the  performance  will  be 
deteriorate.  Thus  the  sampling  rate  cannot  fall  below  2  Hz. 


Figure  5-2  SEMNON  Simulation  Plot  for  h=0.005 


With  the  motor  system  described  in  Chapter  2,  the  controller  is  implemented 
on  the  IBM  PC/AT.  The  implementation  result  is  shown  in  Fig.  5-3. 

Due  to  the  existence  of  ripple  torque,  this  plot  does  not  quite  match  the 
simulation  result  obtained  from  SIMNON  .  This  problem  will  be  discussed  and 
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tine 


o  :  0.085 

Figure  5*3  Implementation  Result  for  h=0.005 
solved  later  (Chapter  6)  . 

I 

Thb  simple  example  illustrates  that  the  integral  controller  can  be  used  to 
control  a  system.  The  main  reason  of  using  an  integral  feedback  is  to  remove  the 
steady  state  error.  By  the  way  just  illustrated,  it  has  another  feature  of  adding  more 
freedom  (design  parameters)  in  doing  the  design.  Thb  idea  will  be  used  later  in 
designing  a  controller  for  the  flexible  arm. 
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CHAPTER  VI 


Identification  of  Actuator  Characteristics 


6.1  Introduction 

The  characteristics  of  an  actuator  (motor)  in  a  robotics  Bystem  affect  the 
performance  of  a  controller  design  in  a  significant  way.  Among  the  many  aspects 
of  a  realistic  system,  there  are  two  items  of  significance:  friction  and  ripple  torque. 
This  chapter  concerned  with  these  two  effects  and  show  how  to  compensate  for  their 
influence. 

In  the  extensive  investigations  of  friction,  Walrath[l]  modeled  the  gimbal 
bearing  friction  as,  in  Dahl’s  model, 

Tj  +  r  ~~~  =  (sgn  u)T„  (6.1) 

where  7/  is  the  total  friction,  Te  is  the  stiction  friction  (a  constant),  and  r  is  a 
parameter  to  be  estimated.  On  the  other  hand,  Canudas[3]  took  the  viscous  friction 
into  consideration.  Here,  a  compromised  model  is  set  up  and  the  experiment  shows 
that  this  new  model  is  more  realistic. 

6.2  Friction  and  Ripple  Torque  Modeling 

6.2.1  Friction 

There  are  essentially  three  kinds  of  frictions  existing  in  a  motor  system:  stiction 
friction,  Coulomb  friction,  and  viscous  friction.  The  stiction  friction  and  Coulomb 
friction  can  be  taken  together  as  bearing  friction  and  may  be  modeled  as  in  Fig.  6-1. 

On  the  other  hand,  the  viscous  friction  is  coming  from  the  motion  of  the  drive. 
It  is  proportional  to  the  angular  velocity  and  can  be  modelled  as  in  Fig.  6-2. 
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Fig.  6-1  Model  of  Bearing  Friction 


Fig.  6-2  Model  of  Viscous  Friction 


From  these  models,  we  may  expect  that  the  bearing  friction  will  be  dominant  at 
low  speed,  and  viscous  friction  will  be  dominant  at  high  speed.  Thus,  by  combining 
these  effects,  we  may  model  the  total  friction  as  a  relation  of  the  curve  as  in  Fig.  6-3. 


The  total  friction  torque  Tj  can  be  then  represented  as 


f  aiu  +  0%,  if  w  >  0; 
\  ajw  —  02%  if  w  <  0, 


(6.2) 


where  ai  and  aj  are  functions  of  w,  0\  and  02  are  stiction  frictions  for  u>  >  0 
and  u)  <  0,  respectively.  In  Section  6.3,  the  experiment  is  carried  out  to  identify  the 
parameters  a  and  0  and  it  is  shown  that  the  model  (6.2)  we  described  is  a  realistic 
one. 


6.2.2  Ripple  Torque 

Due  to  the  limitation  of  the  number  of  magnetic  poles  in  the  motor,  the 
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Fig.  6*3  Model  of  Total  Friction 


magnetic  field  is  not  uniform.  The  torque  constant  thus  depends  on  the  angular 
position  of  the  rotor.  So  a  periodic  torque  known  as  ripple  torque  exists  in  all 
permanent  magnet  DC  motors.  It  will  affect  the  system  performance  especially  in 
the  case  of  direct-drive  systems.  By  moving  the  base  of  the  motor  for  one  revolution, 
one  can  feel  the  torque  fluctuations.  Now  this  problem  will  be  investigated. 

Due  to  the  existence  of  the  back  emf  constant,  the  motor  itself  b  a  stable  system. 
The  steady  state  velocity  should  be  constant  when  we  input  a  constant  current.  But 
the  experiment  shows  that  it  b  never  a  constant,  instead,  it  is  a  periodic  function  of 
position.  Fig.  6-4  is  a  plot  of  velocity  vs.  position  at  the  steady  state  when  we  input 
a  constant  voltage. 

When  the  velocity  b  low,  the  experiment  shows  this  curve  is  almost  a  sinusoidal 
function.  Therefore,  the  ripple  torque  can  be  modeled  as 

Tp  =  k  sin  [a(0  +  0off.«t)]  •  (6.3) 

where  ic,  a,  0off,«t  are  to  be  estimated. 

6.3  Experiment 

6.3.1  Motor  Model  with  Friction  and  Ripple  Torque 

With  the  friction  and  ripple  torque  model  discussed  in  the  previous  sections, 
the  block  diagram  of  the  whole  system  is  shown  in  Fig.  6-5,  which  b  revised  from 
Fig.  2-2. 
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Figure  6-5  Block  Diagram  of  the  Whole  System 


where 

a,  0,  k ,  a,  ^off.et  as  defined  before,  and  Tp  is  the  ripple  torque. 
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From  Eq.  2.1,  the  state  equation  of  the  whole  system  can  be  represented  as 


Jw{t)  =  T(t) 

=  -«»(«)-/» +  !•,(*) 

=  -  [—j^  +  “]WC0  +  -^rpu(i)  -  0  +  k  sin  [o(®  +  9off..t)]  (6.1) 

6(t)=u>(t)  (6.2) 


6.3.2  Friction 

If  we  assume  that  there  is  no  ripple  torque,  the  equation  is 

Jw{t)  =  -  +  a]w(t)  +  “  P 

iL(X 

Then,  at  steady  state,  with  the  constant  input  u(t)  =  €t , 

w(t)  — ►  Q  and  d>(t)  — »  0. 


Thus, 

n  'KtKh  ,  ,  ,  KKt„  0 

Os=_[__  +  a]u>+— -fi-0. 

XI  itg 

Therefore,  by  measuring  the  average  velocity  at  steady  state  (this  take  out  the  effect 
of  the  ripple  torque),  a  can  be  computed  as 

(6.3) 

This  estimated  a  is  in  fact  the  slope  of  the  shaded  line  L  shown  in  Fig.  6-5.  It  is 
not  the  slope  of  the  tangent  line  of  the  T/-u  curve. 

Since  the  parameter  /?  is  exactly  the  stiction  friction  of  the  motor,  it  can  be 
measured  directly  by  applying  a  small  voltage  across  the  motor  while  the  motor  does 
not  move.  Record  the  maximum  value  of  voltage  which  moves  the  motor;  then  /9 
can  be  obtained. 


6.3.3  Ripple  Torque 

If  there  is  no  ripple  torque,  with  constant  input,  the  steady  state  value  of 
the  velocity  should  be  a  constant.  But  the  experiment  shows  that  the  steady  state 
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velocity  is  a  sinusoidal  function  of  position.  By  measuring  the  peak  value,  frequency, 
and  shift  value,  the  velocity  can  be  written  as 


u  =  O  (l  +  k'  sin[e(0  +  0off*et)]) 


(6.4) 


Take  the  Laplace  Transform  of  Eq.  6.1,  without  the  ripple  torque  term,  one  can  get 

TTr„  .  tKtKb  KKtU  fi 

bJW  (s)  =  +  a]W(s)  +-^7-7 

By  some  algebraic  manipulation,  we  reduce  this  to 

KKtU  -  JUP  f  1  _  1  1 

8  KtKb  +  aRa  [a  K,ift+orR.J 


8  + 


JR . 


Taking  the  inverse  Laplace  Transform,  one  can  get  the  step  response  of  the  velocity 
oj(t)  as 


w(t)  = 


KKtU  -  Rap 


KtKb  "f  aRa 

where  h(t)  is  a  unit  step  function.  Thus,  as  t  — ►  00, 

KKtU  -  Ra0 


u[t) 


and 


w  = 


KtKb  +  aRa 
KKtU  -  Rap 


KtKb  +  aRa 

By  the  formula  Eq.  6.4,  taking  ripple  torque  into  account,  we  have 
w(0)  =s  a  (l  +  K* sin  [a(0  +  0off.«t)]) 

=  WZris:  (* +«’»“[«(* + »-«„.)]) 

=  HT+i  V~f )  (I  +  IC' ““[<■(*  +  *•»-)]) 

1  fKKtug  \ 

^  +  Ra  P’r(KKtn  *,  .  ,  J 

*•  _  fijK  sin[o(J  +  0off..t)J 


Ripple  Torque  Term 


Therefore,  the  ripple  torque 


Tp  =  *sin[a(0  +  0off.et)] 
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can  be  found  by 


(6.5) 


K  =  n'(^U-0) 


k  can  be  thus  obtained  by  k'  and  U  —  0). 


6.4  Data  Analysis 


6.4.1  Friction 

First,  the  parameter  P  was  measured.  It  is  essentially  the  stiction  friction  of 
motor.  Because  of  the  existence  of  the  ripple  torque,  it  is  also  a  function  of  position. 
By  averaging  the  voltages  for  starting  the  motor  at  different  positions,  P  can  be 
obtained  as 

Pi  =  1.7334  (Volt)  for  uj  >  0 

Pi  =  1.14  (Volt)  for  oj  <  0 

In  order  to  take  out  the  effect  of  ripple  torque,  the  average  value  of  velocity  is 
also  computed.  Using  the  shaft  encoder  and  the  time  counter  on  the  DASH16  board, 
the  average  velocity  can  be  obtained.  Table  6.1  lists  the  experimental  data  and  the 
calculated  values  of  a . 

Here  a  is  computed  by 


a 


a\  KtKb 

oK(  0)~~R r 


For  the  reason  of  different  scale,  this  is  different  from  Eq.  6.3.  The  output  voltage  U 
is  changed  to  the  input  voltage  to  motor  V .  In  fact,  there  is  a  gain  between  them, 
but  due  to  the  reason  that  there  is  some  offset  voltage  in  the  amplifier  from  C  to  V , 
the  latter  is  more  suitable  here. 

The  relationship  between  the  velocity  and  a  (in  the  unit  of  lb  —  in /rad /sec)  is 
plotted  in  Figure  6-6. 

For  the  reason  talked  before,  a  measured  is  not  the  tangent  slope  of  the  Tf-w 
curve.  The  actual  T/-w  curve  can  be  reconstructed  by  plotting  (u>,au >  +  P)  in  the 
x-y  plane.  It  is  shown  in  the  Fig.  6-7. 
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Input  Voltage 

Displacement 

Time  Interval 

Velocity 

(Volt) 

(rad.) 

(sec) 

(rad  /sec) 

Ot 

2.75 

0.4249 

0.5869 

0.724 

-0.6508 

4.23 

0.78386 

0.5864 

1.3367 

-0.3703 

5.71 

1.135 

0.58736 

1.932 

-0.2477 

7.19 

1.4757 

0.586 

2.51824 

-0.177 

8.61 

1.8208 

0.5882 

3.0956 

-0.1296 

11.63 

1.9896 

0.4685 

4.2467 

-0.0721 

14.59 

2.5372 

0.4689 

5.4109 

-0.043 

17.55 

2.324 

0.353 

6.5835 

-0.026 

20.5 

2.7197 

0.3529 

7.7068 

-0.005 

23.5 

3.11398 

0.3512 

8.904 

0.0011 

26.4 

2.9406 

0.2928 

10 

6.0153 

29.4 

2.637 

0.2348 

11.16 

0.02324 

32.4 

2.901 

0.2354 

12.36 

0.0245 

-3.06 

-0.514 

0.589 

-0.873 

-0.1569 

-4.54 

-0.869 

0.589 

-1.478 

-0.0912 

-6.04 

-1.2087 

0.589 

-2.052 

-0.0354 

-7.51 

-1.547 

0.5881 

-2.6305 

-0.0137 

-9 

-1.883 

0.589 

-3.198 

0.0096 

-11.95 

-2.052 

0.4713 

-4.5354 

0.0256 

-13.42 

-2.32 

0.4712 

-4.923 

0.033 

-14.90 

-1.948 

0.3535 

-5.51 

0.035 

-17.85 

-2.353 

0.3535 

-6.656 

0.043 

-20.8 

-2.765 

0.3535 

-7.822 

0.045 

-23.8 

-3.18 

0.3535 

-8.996 

0.0489 

-26.7 

-3.581 

0.3534 

-10.148 

0.0489 

-29.7 

-2.663 

0.2358 

-11.293 

0.0554 

•32.6 

•2.938 

0.2357 

-12.465 

0.0521 

Table  6-1  Experiment  Data  for  the  Calculation  of  a 


Ftom  the  above  plot,  it  b  clear  that  the  expected  model  (Figure  6.3)  exactly 
matches  the  experimental  result.  From  the  values  of  a  and  the  exact  model  of 
friction  can  be  found  and  used  in  the  compensation  scheme. 
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Figure  6-7  Reconstructed  Tj-w  Curve 


also  done. 

By  using  the  tachometer,  the  peak  value  of  the  velocity  can  be  found.  Figure  6* 
4  is  an  example  of  the  profile  of  velocity  with  respect  to  the  position.  As  illustrated 
in  Section  6.3.2,  first  the  k'  in  Eq.  6.5  is  calculated  by  the  formula 

,  high  peak  value  —  low  peak  value 
2  •  nominal  velocity 

Table  6-2  presents  the  experiment  data  for  the  ripple  torque. 


Input  Voltage 

(Volt) 

Velocity  (rad/sec) 

Kf 

High 

Low 

Nominal 

0.1807 

0.885 

0.16 

0.523 

0.693 

0.205 

0.905 

0.25 

0.578 

0.567 

0.254 

1.025 

0.385 

0.7 

0.457 

0.498 

1.485 

1.07 

1.277 

0.163 

0.742 

2.02 

1.665 

1.8425 

0.096 

1.23 

3.1 

2.82 

2.96 

0.047 

1.719 

msm 

3.95 

4.07 

0.031 

2.207 

5.355 

5.09 

5.22 

0.025 

2.695 

6.46 

6.16 

6.31 

0.024 

3.183 

7.635 

7.375 

7.505 

0.017 

3.672 

8.7505 

8.435 

8.595 

0.019 

4.16 

9.9 

9.63 

9.765 

0.014 

4.648 

11.055 

10.75 

10.9 

0.014 

5.137 

12.195 

11.885 

12.04 

0.013 

Table  6*2  Experimental  Data  for  the  Ripple  Torque 


Figure  6-8  shows  the  relationship  between  k*  and  — 0)  (asinEq.  6.5) 

Assume 

K" 


which  implies 


log  k!  =  log  Cl  +  e2log -  0)- 
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(0,0)  5  ntu-P 

[1%  —  in) 

Fig.  6-8  Relation  between  k'  and  —  fi 


By  the  linear  regression  method  with  the  data  log*'  and  log —  /?),  ej,  e 2 
can  be  found  as 

ci  =  0.0516 

€j  SB  —1 

i.e. 

*'  =  0.0516 {^~U  -  0)~l 

Ra, 

SO, 

(^£7.^  =  0.0516 

This  matches  the  expected  formula  Eq.  6.5.  Thus  the  parameter  k  of  the  ripple 
torque  is 

k  =  0.0516 

Since  there  is  some  time  delay  between  the  data  retrieved  from  the  position 
encoder  and  the  tachometer,  the  peak  point  shown  in  the  velocity  profile  has  been 
shifted.  Thus  the  fi -profile  is  used  to  specify  the  offset  value.  It  is  found  that 

^offset  =  1.509  (rad.) 
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By  plotting  the  velocity  profile  for  a  revolution,  the  period  of  ripple  torque  is 
found  to  be  41.  Therefore,  the  model  for  the  ripple  torque  is,  finally, 

Tripp]*  =  0.0516  sin  [41  (9  +  1.509)]. 


6.5  Compensation  Scheme 

With  the  analysis  and  discussion  in  the  previous  sections,  now  the  compensator 
for  their  effects  will  be  established. 


6.5.1  Friction  Compensation 

The  friction  in  a  motor  system  may  be  modelled  as  in  Fig.  6-3  as  described 
before.  Due  to  the  fact  that  nonlinear  functions  take  a  lot  of  time  in  computing,  an 
approximate  piecewise  linear  model  for  the  friction  is  chosen  as 


where 


TfiW  +  0J, 

<*l“>  +  0l, 
0, 

(*2U  -  &  > 

W  -  0£, 


w  >  u>i; 

W!  >  a;  >  0; 

u  =  0; 

0  >  w  >  a>2j 
u>2  >  u>; 


<*1,02,  <  o, 


01, 02,  #,02,71,  72,  Wl  >  0, 

are  all  constant.  The  profile  of  this  friction  model  with  respect  to  velocity  is  shown 
in  Fig.  6-9.  The  friction  torque  first  decreases  and  then  increases.  It  means  that,  at 
first,  the  Coulomb  friction  dominates  and  after  the  critical  point,  the  viscous  friction 
dominates. 


By  using  the  feedback 


in  the  original  model 


J  =  u  + 


tL 

Kt 


Ju  =  KtI  -  Tf 


where  the  Tf  is  the  true  friction,  the  overall  state  equation  will  be 


Ju>  =  Ktu+(ff-Tf). 
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Figure  6-9  Approximated  Model  for  Friction 


Thus,  if  the  model  is  “good”  enough,  i.e.  f'j  «  T/,  then 

Joj  =  Ktu ,  where  u  is  the  current  input. 

In  order  to  show  that  the  above  friction  compensator  works  well,  a  velocity  tracking 
problem  is  attacked.  In  the  following,  an  algorithm  for  speed  control  is  firBt 
illustrated,  then  the  compensator  will  be  included  in  that  algorithm.  The  experiment 
shows  that  this  compensator  makes  the  system  response  faster. 

Considering  the  motor  system  described  in  Sec.  2.2,  the  state  equation  is 


(6.15) 


Let  ojr  be  the  reference  input  (velocity  profile),  the  set-point-on-I-only  controller  [2] 


is  used  as 


U{t)  =  Kp  j-w  +f:J0  WT)  “  w(r)] rfr}  • 


(6.16) 


Plug  the  above  expression  for  U  (t)  into  the  Eq.  6.15,  to  get 

_ . .  .  (KKtKp  Kt  \  ,,  KKtKp  f*  „  . 

M‘)  =  -  (-*-*  +  xjci)  -W  +  -wr!0  MO  —Ml  *• 

Taking  the  derivative  of  both  sides, 

(KKXKP  .  Kt  \.  ,  KKtKp ,  M 

M‘)  =  -  (-^ + Rjrj  « + -gjf-  m«)  —  Ml- 

Furthermore,  taking  the  Laplace  Transform,  one  can  get 

[j.’  +  KK‘K£^*'‘  +  "M  = 
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Thus,  the  transfer  function  from  a ;r  to  u  can  be  written  as 


G(s)  = 


KKtKp 

RaTi 


2  ,  KKtKbKp  +  Kt  ,  KKtKp 


Js*  + 


RaKb 


«  + 


R*Ti 


With  this  continuous  linear  system  and  letting  Kp ,  be  design  parameters, 

CONSOLE  may  be  used  to  do  the  design  (tuning  the  gains.) 

Appendix  B  lists  the  input  file  for  CONSOLE  in  this  problem.  Instead  of 

using  the  time-domain  representation,  the  frequency-domain  representation  (transfer 

function)  is  used  in  the  system  description  file. 

The  values  of  design  parameters  and  the  Pcomb  output  are  shown  below. 

Nam*  Value  Variation  vrt  0  Pr*v  It*r-0 

Kp  1.07312**00  1.0**00 

Ti  2 .033400-01  1.0**00 

Pcomb  (It*r-  0)  (Phaa*  2)  (ops-  i.000**00)  (MJUC.COST.BOFT-  0.807208) 

SPECIFICATION  PRESENT  GOOD  C  B  BAD 

FOi  speed  1.00*+00  1.01**00  — — — •  |  J  1.05**00 

F02  underehoot  0.17e-01  0.00* -01  |  ———————  0.00* -01 

The  step  response  of  the  closed-loop  system  plotted  by  CONSOLE  is  shown 
in  Fig.  6-10. 

After  getting  these  parameters,  the  next  thing  is  to  translate  the  controller  into 
discrete-time.  The  s -domain  equation,  from  Eq.  6.16, 

V(e)  =  -KrW(.)  +  -  W«] 


may  be  transformed  into 

u[kh]  =  -A>/[kh]  +  ^^~-(u;r[kh]  -w[kh]) 

by  using  Euler  Approximation  in  the  integral  part.  In  order  to  avoid  the  windup 
(integrator  saturation)  problem  [2],  the  velocity  form  is  chosen  in  getting  the  control 

«[kh]  =  «[(k  -  i)h]  -  if,{w[kh]  -  w«k  -  i)h]}  +  ^Kt(k  -  i)hj  -  w[(k  -  i)h]} 

with  tt[0]  =  0. 
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Function*!  Objactlv*  ipttd 


xlO® 


Then  SIMNON  is  used  to  simulate  the  whole  system.  The  input  file  for  it 
is  also  listed  in  Appendix  B.  By  choosing  the  sampling  time  to  be  0.002  sec,  the 
simulation  plot  is  shown  in  Fig.  6-11. 

Next  step  is  doing  the  implementation.  The  real-time  program  was  written  in  C 
language.  Fig.  6-12  gives  the  experimental  comparison  of  controllers  with  and  without 
friction  compensation.  The  sampling  period  is  0.002  sec  (i.e.  for  each  iteration.)  The 
scheme  with  the  friction  compensator  really  improves  the  performance. 

6.5.2  Ripple  Torque  Compensation 

From  Sec.  6.4.3,  the  ripple  torque  Tv  can  be  modelled  as 

Tp  =  0.0516  sin  [41(0  +  1.509)]. 

Thus,  by  using  the  compensator 

«(t)  =  v(t)  -  (0.0516 sin  [41(0+  1.509)1) 

A  At 

in  Eq.  6.4,  the  ripple  torque  can  be  reduced. 
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1.2, 


1. 


Figure  6-11  Simulation  by  SIMNON  for  the  Velocity  Tracking  Problem 

But  due  to  the  computing  time  between  getting  data  from  the  position  encoder 
and  issuing  the  control  signal,  there  is  a  computational  delay [18]  .  In  order  to  handle 
this  problem,  it  is  necessary  to  measure  the  computing  time  accurately.  In  Sec.  7.2, 
the  computing  times  for  some  functions  in  Microsoft  C  have  been  compared.  Based 
on  that  discussion,  the  code  may  be  “optimized”  in  the  sense  of  shortest  computing 
time.  The  compensator  is  thus  implemented  on  the  IBM  PC  in  a  Microsoft  C  program 
as  shown  below. 

■In*  *  »in(  41  *  (po»d»ta[i)  ♦  time  *  ▼•ldata[i]  -  offset)  ); 

Volt[i]  *  motref  •  k  •  else; 
aotlnp  -  Volt  [1]  •  204.8  ♦  2008; 
dd*6(  Botlnp  ); 

Here  time  is  the  measured  time  period  between  obtaining  position  data  and  sending 
the  control  signal.  Fig.  6-13  shows  the  plot  of  velocity-vs-position  after  compensation. 
Compared  with  Fig.  6.4,  it  shows  that  the  ripple  torque  problem  can  be  solved  using 
explicit  compensation. 

Since  the  effect  of  ripple  torque  is  reduced  as  the  load  inertia  increases,  for  a 
heavy  arm,  it  can  be  neglected.  But  for  a  light-weight  arm,  as  in  our  case,  it  should 
be  properly  compensated  for.  Sec.  7.3  will  include  the  compensator  in  the  control  of 
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CHAPTER  VII 


Implementation 


7.1  Hardware 

7.1.1  D/A  and  A/D  Converter 

The  D/A(digital-to-analog)  converter  chosen  in  the  experiment  is  the  DDA06 

board  manufactured  by  Metrabyte  Company.  DDA06  is  an  analog/digital  I/O 

expansion  board  for  the  IBM  PC.  It  provides  six  channels  of  12  bit  analog  output 

and  24  lines  of  digital  I/O.  The  8255  programmable  peripheral  interface  chip  is  used 

for  digital  I/O  and  can  be  operated  in  any  mode  (straight  I/O,  strobed  I/O,  and 

bidirectional  I/O).  In  this  experiment,  one  D/A  channel  is  used  to  send  a  voltage 

signal  (+10/- 10  Volt)  to  motor  servo  electronics  and  16  lines  of  digital  I/O  are  used 

to  get  the  12  bit  data  from  the  position  encoder.  The  I/O  operations  of  DDA06  are 

essentially  the  port  (address)  manipulations.  The  function 

ddiS(  data  ) 
int  data; 

{ 

lnt  baaedda6,  chan; 
lnt  xh,  xl; 
baaaddaO  ■  788; 
xh  ■  data/256; 
xl  ■  data  -  xh  *  256; 
outp(  baaadda6,  xl) ; 
outp(  ba«*dda6+l,  xh) ; 
return; 

> 


sends  a  voltage  through  DDA06  by  an  integer  between  -2048  and  2047,  where 

basedda6  is  the  base  address  of  DDA06  board.  The  lines 

outp(  ba>*dda6+15,  Oxba  ); 

/****•  Input  pealtion  data  •**••/ 
pa  •  lnp(  baaadda6+12  ); 
pb  ■  inp(  baaadda6+13  ); 

poadata  ■  ((pb*256+pa)  »  4) *3. 141602653  /  2048; 
set  the  operation  mode  1  for  the  8255  chip  and  read  position  data  (from  the  shaft 
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encoder)  in  the  unit  of  rad. 

Another  of  Metrabyte’s  products,  DASH16  is  used  as  the  A/D  (analog-to- 
digital)  converter.  It  provides  eight  differential  channels  each  with  a  12  bit  successive 
approximation  converter  with  a  25  fia  conversion  time,  a  3  channel  programmable 
interval  timer  (INTEL8253),  and  some  other  features  (see  [6]).  In  our  experiment, 
two  analog  signals  ranging  over  4-10  to  -10  (Volt)  from  the  tachometer  and  the 
accelerometer  are  converted  to  integer  numbers  ranging  over  +2047/-2048  by  the  two 
channels  in  DASH16.  The  timer  on  DASH16  is  used  to  set  the  sampling  period  for 
the  control  loop.  With  the  software  written  by  manufacturer  and  the  interface  which 
will  be  discussed  later,  the  DASH  16  board  can  be  easily  used  in  the  programming. 
The  following  is  a  subroutine  of  initializing  the  DASH16  board  and  also  the  counter 

inside  it. 

InltdIOO 

node  ■  0; 

dlfiio  [0]  -  beeedlfi; 
dlfiio [1]  •  7; 
dlfiio  [2]  ■  1; 
flag  ■  0; 

dashlfi (  Innod# ,  dlfiio,  flag  ); 
lf(  flag  >  0  )  error (  flag,  noda  ); 

/***#*  Setup  the  Counter  0  eeeee/ 

outp(  beaedlfi+10,  2);  /•  Set  the  Cl“l  C0«0  */ 

mode  *  10; 

dlfiio [0]  ■  2;  /*  Configuration  2  */ 

flag  »  0; 

daehlfi(  tanode ,  dlfiio,  fcflag  ); 

if  (  flag  >  0  )  error (  flag,  node  ); 

mode  “  IS; 

dlfiio [0]  »  4;  /a  Vrlte  0P2  ■  1  */ 

dashlfi(  too de,  dlfiio  ,  ftflag  ); 
if  (  flag  >  0  )  error(  flag,  node  ); 


7.1.2  Handshaking 

In  order  to  set  up  the  connection  between  the  programmable  peripheral 
interface  8255  residing  on  the  DDA06  board  and  the  encoder,  it  is  necessary  to 
set  up  a  handshaking  scheme.  The  strategy  is  as  follows. 

1.  Program  sends  a  RD  (ReaD)  signal  to  8255.  (e.g.  by  the  inpO  function  call 
in  C) 

2.  8255  sets  the  IBF  (Input  Buffer  Full)  to  be  low  and  also  send  a  BICTS  (Binary 
Clear  To  Send)  signal  to  the  encoder. 
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3.  After  receiving  BICTS  from  8255,  encoder  sends  a  BISTROBE  (Binary 
STROBE)  signal  to  8255  and  sets  the  STB  (STYoBe)  of  8255  to  be  high  and 
then  loads  data  into  8255 

4.  8255  sends  the  data  to  the  variable  in  the  program  and  drives  the  IBF  to  high, 
and  also  STB. 

The  operation  mode  1  (strobed  I/O)  of  8255  is  used.  The  configuration  between 
8255  and  the  encoder  servo  electronics  is  shown  in  Fig.  7-1.  The  timing  between  those 
signals  is  shown  in  Fig.  7-2.  With  this  scheme,  the  program  can  get  the  shaft  encoder 
data  without  any  error. 


Figure  7-1  Configuration  between  Encoder  and  PPI8255 


7.1.3  Low-Pass  Filter 

Since  there  is  high  frequency  noise  coming  from  the  analog  sensors,  i.e.  the 
tachometer  and  the  accelerometer,  it  is  necessary  use  low-pass  filters.  On  the  other 
hand,  because  the  differential  signals  from  the  sensors  do  not  have  the  same  ground 
with  the  DASH  16  board,  it  is  also  necessary  to  design  a  voltage  follower  (buffer)  for 
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5dT 
ibfa 
RdI 

IBFb 
BICTS 
BISTROBE 
STlT 
STBb 

Loading  B 

Figure  7-2  Timing 


the  differential  signal.  The  circuit  diagram  for  the  filter  and  the  buffer  is  shown  in 
Fig.  7-3.  The  potentiometer  is  used  to  adjust  the  offset  value  of  the  amplifiers. 


Figure  7-3  Low- pass  Filter  and  Voltage  Follower 


The  cutoff  frequency  is 


_ 1 _ 

2  *  jt  *  10000  *  0.018  *  10“® 


=  884  (Hz) 


From  the  experiment,  it  is  found  that  the  errors  from  the  sensors  reduced  from  ill 
bits  to  i3  bits. 
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7.2  Software 


7.2.1  Interface  between  DASH16  and  C  language 

For  the  DASH16  board,  there  Is  a  software  package  DASH16.0BJ  designed  by 
the  manufacturer  which  is  originally  written  in  BASIC.  In  order  to  use  it  within  a  C 
program,  two  problems  have  to  be  solved: 

1)  The  Microsoft-C  compiler  automatically  adds  an  underscore  before  any 
function  call  when  it  generates  the  object  code  for  a  C  routine.  For  BASIC, 
there  is  no  such  addition. 

2)  The  way  to  process  the  argument  of  a  function  is  different.  For  C,  the  arguments 
are  pushed  into  a  stack  from  left  to  right,  but  for  Basic,  the  order  is  from  right 
to  left. 

Thus  it  is  necessary  to  write  an  Assembly  language  subroutine  to  set  up  the 
connection.  The  C  programs  then  call  the  assembly  routine,  and  the  assembly  routine 
calls  the  object  code  DASH16.0BJ. 

The  following  List  7-1  is  the  listing  of  the  assembly  interface  code.  It  includes 
a  named  “jdashl6”  function  which  calls  the  function  “dash  16.” 


List  7-1  Listing  of  Interface  Assembly  Code 
TITLE  interface 

.TEXT  SEGMENT  BYTE  PUBLIC  'CODE' 

.TEXT  ENDS 

.DATA  SEGMENT  WORD  PUBLIC  'DATA' 

.DATA  END8 

CONST  SEGMENT  WORD  PUBLIC  ’CONST* 

CONST  ENDS 

.BSS  SEGMENT  WORD  PUBLIC  *BSS’ 

.BBS  ENDS 

DGROUP  GROUP  CONST.  .B88,  .DATA 

ASSUME  CS:  .TEXT.  DS:  DGROUP,  SS:  DGROUP.  ES:  DGROUP 
EXTRN  DASH1A:FAR 
.TEXT  SEGMENT 

PUBLIC  _daahl« 

.daahifi  PROC  NEAR 
puah  bp 
aov  bp,  ap 
xor  ax,  ax 
aov  ax,  [bp+4] 
puah  ax 
aov  ax ,  [bp+fl] 
puah  ax 
aov  ax,  [bp +8] 
puah  ax 
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call  DASH16 
■ot  ap,  bp 
pop  bp 
rat 

.dishie  ENDP 
.TEXT  ENDS 
END 


7.2.2  Computing  Time 

In  a  real-time  system,  the  computing  time  is  crucial  in  design.  In  order  to 
have  some  idea  of  how  much  time  is  spent  in  evaluating  some  specific  functions, 
the  computing  time  for  them  has  been  estimated  by  using  the  timer  counter  on 
the  DASH16  board.  Table  7.1  gives  the  computing  time  for  some  commands  in  C 
language.  It  reveals  some  basic  facts  in  real-time  computing. 


Integer  Operations 

Time  (/is) 

Floating  Point  Operations 

Time  (/is) 

Assignment 

Constant 

2.1 

Assignment 

Constant 

47.5 

Variable 

small 

Variable 

8 

Addition(+) 

Constant 

0.1 

Addition  (  +  ) 

Constant 

0.5 

Variable 

0.4 

Variable 

9 

Multiplication  ( x  ) 
Constant 

0.1 

Multiplication  (  x  ) 

Constant 

0.5 

Variable 

0.6 

Variable 

10 

Shift(») 

0.4 

Functions 

Modulus(%) 

0.6 

printfQ 

66500 

Bitwise  AND(&) 

0.3 

sin() 

874 

Bitwise  Exclusive  OR(*) 

0.3 

asinQ 

603 

Functions 

sinhQ 

801 

sizeof() 

2.2 

pow() 

971 

abs() 

19.4 

fabsQ 

35 

Port  I/O 

exp() 

600 

outp() 

17 

log() 

539 

sqrtQ 

187 

Table  7.1  Computing  Time  for  some  C  Function 
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*  Except  for  the  assignment  command,  the  time  used  for  variable  operations  is 
much  more  than  for  constant  operations.  So  use  constant  operation  if  possible. 

*  If  the  assignment  command  is  desired  for  constant,  e.g.  T  =  0.012,  it  is 
better  to  define  that  constant  by  another  variable  outside  the  control  loop,  e.g. 
5  =  0.012,  then  use  T  =  S  in  the  control  loop. 

*  I/O  functions  cost  a  considerable  penalty  in  time.  Avoid  them  in  the  control 
loop  in  any  case. 

*  Integer  operations  always  take  very  little  time.  Use  them  rather  than  floating 
point  operations  whenever  possible. 

These  Tacts  of  life”  should  be  kept  in  mind  in  the  real-time  programming. 

7.3  Controller  Implementation  for  a  Flexible  Arm 

Following  the  insights  of  Chapter  6,  an  integral  controller-observer  scheme  was 
implemented  for  control  of  the  flexible  arm,  with  the  system  described  in  Section  2.1. 
The  design  procedure,  as  illustrated  in  the  Chapter  1,  is  as  follows, 

(1)  designing  the  state-integral  feedback  gain  by  using  CONSOLE 

(2)  translating  the  continuous-time  design  to  discrete-time 

(3)  designing  a  discrete-time  observer 

(4)  simulating  the  whole  system  using  SIMNON 

(5)  implementing  the  scheme  in  real-time  system 

(6)  adding  the  compensator  for  friction  and  ripple  torque 

Besides  the  step  (3),  which  exactly  follows  the  discussion  in  [ll],  each  step  will 
be  described  in  the  following  sections. 

7.3.1  Designing  the  state-integral  feedback  gain  by  using  CONSOLE 
As  discussed  in  Sec.  4.2  and  Chapter  5,  CONSOLE  is  used  to  design  the 
feedback  gain  from  each  state  of  the  system  and  the  integral  of  the  plant  output. 
In  order  to  preserve  the  controllability  and  observability  properties,  the  conditions 
in  Proposition  5-1  and  5-2  should  be  satisfied.  Since  here  an  observer  is  used  to 
reconstruct  the  states,  these  constraints  are  important.  Appendix  C  lists  the  input 
file  for  CONSOLE  .  The  constraints  named  “control”  and  “observ”  are  used  for  the 
purpose  mentioned  above. 
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The  gains  designed  from  CONSOLE  and  Pcomb  output  are  listed  as  follows. 


Nama 

Valua 

kl 

8.23404e+OO 

k2 

-1.683469-01 

k3 

-1.670009-03 

k4 

2.387009-06 

k6 

4.080049+01 

k0 

6.86300a-01 

kT 

-2.386809-00 

a 

1.21241a+01 

P 

1.07714a+01 

Variation  art  0  Prav 

1.4a +00 

0.0a-02 

8. 4a -04 

l.Ta-06 

4.3a+01 

2.0a-01 

1.0a -Ofl 

E.Oa+OO 

S.Oa+Ol 


ltar«0 


Pcomb  (Itar-  0)  (Pbaaa  2)  (epa«  l.OOOa+OO)  (HAJ.COST.SOFT-  0.034043) 


SPECIFICATION 

PRESENT 

COOD 

01 

ateadystat 

0.03e-O8 

0.00e+00 

F01 

ovar-ahoot 

1.00a+00 

l.Olo+OO 

Fo2  Battling 

O.Ola-Ol 

0.00a-01 

Cl 

obaarvl 

1.07a+00 

1.00a-02 

C2 

obaarv2 

1.02a+00 

1.00a-02 

C3 

obaarvS 

0.109-01 

1 .00a -02 

C4 

controll 

T. 04a+00 

1.00a -02 

FC1 

maz-accal 

2.71a+00 

1.00a+01 

FC2 

maz-accal 

-0.71a-Ol 

-l.OOa+Ol 

FC3 

control 

1.03a+00 

4.00a+00 

C  B 

tllSEKZStSeiCUZHSina 

< - 

< - 

< - 

< - - 

<--  I  I 

< - - - — - - — -- — - 

<--  I  I 


BAD 

1.00a -04 
1.10a+00 
0.60b-01 
1.009-04 
1 .00a -04 
1.00e-04 
1.00a-04 
1. 109+01 
-l.lOa+Ol 
6.00a+00 


The  simulation  of  the  continuous  system  with  those  gains  is  shown  in  Fig.  7-5. 


Functional  Objactira  ovar-ahoot 


xl0° 


Figure  7-5  Simulation  of  the  Flexible  Arm  System  from  CONSOLE 
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7.3.2  Translating  the  Continuous-time  Design  to  Discrete-time 


A  linear  time-invariant  state  equation  may  be  written  as 


x  =  Ax  +  B  u 


y  =  Cx 


This  continuous-time  equation  can  be  translated  to  discrete-time  as 


s[(k  +  l)h]  =  #x[kh]  -f  ru[kh] 
y[kh]  =  Cx[kh] 


where 

9  =  exp  (Ah) 
r  as  f  exp(Ar)  dr  B 

Jo 

In  finding  the  exponential  of  a  matrix,  many  methods  can  be  used.  Since  just  one 
computation  is  needed  here,  the  most  straightforward  way  is  chosen,  i.e.  the  series 


expansion  of  it, 


*  F  ,  ,  AW  ,  AW  , 
9  =  I  +  Ah  H — 1 — 1- 


2! 
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By  using  a  simple  routine  in  MACSYMA  a a 

noloop: 100; 

EXP  A : ldont (N) ; 

AH : ldont (N) ; 

for  1:1  stop  1  thru  noloop  do 
(AH :h*A. AH/1, 

EXPA : EXP A+ AH 
); 

EXPA; 


9  can  be  computed  as  accurately  as  we  need. 
Similarly,  T  may  be  obtained  from 


-XV 


21 


3! 


-)drB 


ABha  AaBh3 
=  Bh+  ——  +  — - —  + 


2! 


3! 


Also,  a  routine  in  MACSYMA  of  the  form 

noloop : 100 ; 

Gum&o:h*B; 

AB :h*B ; 

for  1:1  stop  1  thru  noloop  do 
(AB:h*A.AB/ (!♦!). 
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Gamma : Gamma ♦AB 
); 

Gamma ; 

may  be  used  to  compute  T. 

Other  packages,  such  as  MATLAB,  CC,  have  also  been  tried  and  we  have  found 
that  the  method  used  here  is  the  more  reliable  one. 

In  translating  the  feedback  law 

u  =  —Kx  4-  K\  J  y 

to  discrete-time,  the  same  structure  used  in  Sec.  5.4  is  chosen  here  for  the  integral 
part.  For  the  state  feedback  part,  the  formula  in  [2]  is  used  as 

K  =  K[l+{A-BK)^]. 

Thus,  the  discrete  feedback  law  is  as 

«[kh]  =  -Kx[lh]  +  JT^fkh] 

with  the  formula  for  * 

*[H>!  =  -  i)h]  +  =(1  -  «-■*)*  i[(i  -  i)b], 

since  xi  is  exactly  the  tip  position. 


7.3.3  Simulating  the  Whole  System  by  Using  SIMNON 
The  structure 
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is  used  to  do  the  simulation  with  SIMNON  ,  where  the  plant  is  the  continuous 
system  and  controller  includes  the  discrete-time  observer  and  the  feedback  law.  The 
input  system  description  file  for  SIMNON  is  listed  in  Appendix  C.  With  the  sampling 
rate  of  100  Hz ,  the  tip  position  profile  is  shown  in  Fig.  7-6. 


Figure  7-6  Simulation  with  the  Whole  System  from  SIMNON 

By  choosing  different  sampling  rate  with  SIMNON  ,  it  shows  that  the  sampling  rate 
cannot  be  below  50  Hz.  Needless  to  say,  for  different  sampling  rates,  the  discrete¬ 
time  observer  used  is  different. 

7.3.4  Implementing  the  Scheme  in  Real-time  System 

The  controller  discussed  before  has  been  implemented  in  the  real-time  system 
with  the  controller  residing  in  an  IBM  PC/AT.  The  whole  system  is  shown  in  Fig.  2- 
1.  Some  key  points  used  in  the  real-time  programming  have  been  discussed  before. 
One  other  thing  need  to  be  noted  is  that,  for  the  reason  that  the  saturation  problem 
in  the  D/A  converter  and  amplifier,  a  saturator  is  used  to  the  input  signal  to  the 

observer  [18]  as 

ujiigfe  •  2000; 
u.lov  ■  -2000; 
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if (  u.force  <  n.lov  )  n.forc*  «  n.lov: 

•  1*«  if(  u.forc*  >  u_high  )  n.forc*  ■  n.hlgh; 

•Is* 

With  the  software  written  in  Microsoft  C  (the  code  is  listed  in  Appendix  D), 
the  hub  position  profile  from  the  shaft  encoder  is  shown  in  Fig.  7-7. 


Figure  7-7  Hub  Position  Profile  from  Position  Encoder 


7.3.5  Adding  the  Compensator  for  Friction  and  Ripple  Torque 
The  compensators  for  friction  and  ripple  torque  have  also  been  added  to  the 
controller.  The  new  model  for  ripple  torque  (since  the  load  inertia  has  changed  with 
the  addition  of  the  arm),  is  now 

Tp  =0.18  sin  [41(0-  0.62)]. 

Fig.  7-8  shows  the  performance  of  the  integral  control  with  the  ripple  torque 
compensator.  Since  that  compensator  will  add  as  well  as  reduce  the  speed  during 
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the  motion,  the  rise  time  was  not  changed  much.  But,  when  the  beam  settles  down, 
the  settling  time  and  the  steady-state  error  are  both  reduced  much. 


iter 


o  :  gain:  0.2  offset:  11 


Figure  7-8  Hub  Position  Profile  after  Compensation  for  Ripple  Torque 


The  friction  with  the  flexible  arm  attached  can  be  modeled  as 

'  0.02 u  +  40,  ut  >  1.12; 

— 0.115u;  +  59,  1.12  >  u  >  0; 

Tf  =  0,  u  =  0; 

—0.5a;  —  41,  0  >  w  >  —1.04; 

>  0.021a;  -  30,  -1.04  >  a/; 

Fig.  7-9  shows  the  performance  of  the  integral  controller  with  both  compensators. 
The  improvement  is  quite  evident.  The  rise  time  is  reduced  from  0.6  sec  to  0.4  sec 
and  there  is  almost  no  overshoot. 

The  experimental  results  shows  that  the  characteristics  of  the  actuator  axe 
really  important  in  the  design  of  the  controller  of  a  robot,  especially  as  in  our  case, 
a  direct-drive  low-inertia  system. 
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iter 

o  :  (250  -0.06  0. 05) (-250  -0.06  0.05) 

Figure  7-9  Hub  Position  Profile  after  Compensation  for  Both  Characteristics 
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CHAPTER  VIII 


Conclusion 

Our  work  demonstrates  that  the  integration  of  the  ideas  in  Chapter  4,  5,  and  6 
leads  to  striking  improvement  in  controller  performance.  These  ideas  can  be  carried 
over  to  other  situations  to  design  good  motion  controllers.  Our  work  also  shows 
that  the  investigation  of  the  characteristics  of  the  actuator  is  important.  Every 
drive  system  should  include  compensators  for  nonlinearities  such  as  ripple  torque 
and  friction. 

Although  the  geometrically  exact  mathematical  model  derived  in  Chapter  3 
was  not  used  in  doing  the  design  here,  it  paves  the  way  in  the  future  to  analyze  it 
and  design  a  suitable  controller  for  it.  This  is  definitely  a  challenging  task.  In  fact, 
it  has  been  started.  The  idea  of  modeling  the  beam  as  a  chain  of  rigid  bodies  has 
been  suggested  and  tried.  This  can  be  regarded  as  an  approximation  of  the  exact 
beam  model.  The  existence  and  uniqueness  properties  of  the  exact  equation  should 
also  be  studied. 

Another  interesting  idea  is  to  use  piezoelectric  material,  such  as  PVF2,  to 
increase  the  beam  damping  by  active  control  and  make  it  more  robust.  Because  of 
the  distributed  structure  of  the  film,  the  exact  beam  equation  should  be  used.  This 
is  also  a  next  step  to  consider  for  experimentation. 
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Appendix  A 


Microsoft  C  Program  Listing  of  the  integral  Controller 

for  a  Motor  System 


/*  integral  control  of  the  motor  system  */ 

/*************** . *********** . . 


•include  <process.h> 

•include  <conio.h> 

•include  <stdllb.h> 

•include  <stdio.h> 

•include  <math.h> 
extern  lnt  dashlfiO; 
extern  int  ddafiO; 
extern  void  exltO; 

maln(  argc,  argv  ) 
int  argc; 
char  *argv[]; 

i 

int  mode,  basedlfi*>784.  dl01o[6],  flag,  loadcntO; 
lnt  baseddafi>768,  pa,  pb,  ddafiof st»2006 : 
int  mot.lnp,  posref,  noloop,  nolter; 
lnt  i,  J: 

double  position [1600] ,  z[1500]; 

double  veldata,  posdata.  postole,  posofst; 

double  uf.  cur .force,  vforce; 

double  velgaln,  dlfiofat,  D.A,  A.D,  velfkgn; 

double  contfdbk,  u.force,  p,  m,  h,  exphp,  xy ; 

/***•*  Initialization  of  dashlB  •****/ 

InitdlfiO ; 

/****•  Start  Execution  *•***/ 
ddaO(  ddafiof st  ); 
prlntfC  Start  execution\n") ; 
mot.lnp  «  ddafiof st; 
ddafi(  mot.lnp  ); 

print! ("  Input  step  size  (  10  or  less  )  "); 

scanfC'Xd",  Aposref  ); 
prlntfC  M  Input  no  of  iterations  "  ); 
scan! ("Id",  Anolter  ); 

/****•  Input  Initial  position  data  •**•*/ 
pa  ■  lnp(  baseddafi+12  ); 
pb  ■  inp(  baaeddafi+13  ); 

posofst  ■  ((pb  •  256  *  pa)  »  4)  •  S. 1415926  /  2048; 
/****•  Initialization  of  the  loop  ••*•*/ 
noloop  ■  1000; 
posdata  ■  0; 

▼eldata  ■  0; 
h  ■  0.005; 
p  ■  26.3078; 
m  -  13.7626; 
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•rphp  ■  arp(  -  h«p  ) : 

*y  *  n  *  <1  -  tiphp)  /  p; 

/*••••  start  tho  loop  •««••/ 
for(  1  *  0;  1  <  noltor;  )  < 

*C0]  ■  0; 

poaltlon[0]  ■  0: 

for(  J  ■  1;  J  <  noloop;  J**  )  < 

/***  Load  Countar  0  */ 

nodo  *  11; 

dlfilo  [0]  *  600;  /*  Load  Countar  0  1000  */ 
dashlfiC  fcmodo,  dlfilo  ,  tflag  ); 

If  (  flag  >  0  )  orrorC  flag,  soda  ); 
cur.forco  ■  poarof; 

/**«•*  Input  roloclty  *•••*/ 
nodo  »  1; 
dlfilo  [0]  «  0; 
dlfilo [1]  ■  0; 

daahlfi(  fcnodo,  dlfilo,  tflag  ); 
lf(  flag  >  0  )  orrorC  flag,  nodo  ); 
nodo  ■  S; 

dashlfiC  fcmodo,  dlfilo,  fcflag  ); 
lf(  flag  >  0  )  orrorC  flag,  nodo  ); 
valdata  -  dlfilo [0]  *  3.426  /  204.8; 

/aaaaa  Input  position  data  aaaaa/ 
pa  «  lnp(  basoddafl+12  ); 
pb  ■  lnp(  basoddafi+13  ); 

posdata  ■  CCpb*25fi*pa)  »  4) *3 . 1416026/2048-posof st ; 
position  [J]  ■  posdata; 

/aaaaa  Computing  tho  artificial  atato  aaaaa/ 
i[J]  ■  oxphp  *  «[J-1]  ♦  *jr  •  position [j-1]  ; 

/aaaaa  Computing  tho  foodbaek  Talus  aaaaa/ 

contfdbk  -  8 . 461B8*posdata-0 . 140606*yaldata- 14 . 2800** [j] ; 

u.forco  ■  cur.forco  -  contfdbk; 

nf  *  u.forco  o  204.8; 

lf(  uf  >  2040  )  uf  -  2040; 

lf(  uf  <  -2040  )  uf  ■  -2040; 

not.lnp  «  uf  *  2048; 

ddafiC  not.lnp  ); 

nodo  ■  12; 

dlfilo [0]  ■  1;  /•  Latch  boforo  road  */ 

dashlfiC  fcmodo,  dlfilo,  tflag  ); 
if  C  flag  >  0  )  orrorC  flag,  nodo  ); 
vhilo(  dlfilo [1]  >  10  )  { 
nodo  ■  12; 
dlfilo  [0]  ai  1; 

dashlfiC  ftrnodo.  dlfilo,  tflag  ); 

If  (  flag  >  0  )  orrorC  flag,  nodo  ); 

> 

> 

poarof  ■  -poarof; 

> 

/aaaaa  Tho  End  ••*••/ 
ddafi(  ddafiofat  ); 
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Appendix  B 


Input  File  for  CONSOLE  and  SIMNON 
in  the  Speed  Control  Study 


/************  ** ******************************** ****************** 
Problem  Description  File  for  CONSOLE 
•**************«*******************•*********»******«**********•/ 
design.parameter  Kp  lnit*1.07312e+00 
design.parameter  T1  lnit*2.03340e-01 
/*  Meet  the  engineering  specif ication  */ 
functional.obj active  "speed" 
for  t  from  0  to  1  by  0.001 
minimize  { 

double  Ytr () ; 
return  Ytr C"z",t) ;  > 
good.curve  ■  < 

lf(  t  <*  0.6  )  return  1.1; 
else  return  1.01;  > 

bsd.curve  -  { 

ifC  t  <*  0.6  )  return  1.2; 
else  return  1.05;  > 

functional.obj active  "undershoot" 
for  t  from  0.4  to  1  by  0.001 
maximize  { 

double  YtrO; 
return  Ytr ("z",t) ;  ) 
good.curve  “  { 

if (  t  <■  0.5  )  return  0.00; 
else  return  0.00;  > 

bsd.curve  ■  { 

lf(  t  <*  0.6  )  return  0.85; 
elee  return  0.06;  > 

•tttttif ttttttSttttf ttf f tttf Stttfttttttt# 

•  System  Description  File  for  CONSOLE 
••t«sttsstt*sss«t#s«tts«*s*s«*sssM«t«t«« 

K  -  6 

Xt  »  21.62 
Kb  -  2.443 
Xa  ■  33.6 
J  -  0.1 

subsystem  plant  ■  [  (0)  K*Kt*Kp/(Ra*Ti) ;  \ 

(2)  J,  (K*Kt*Kb*Kp*Kt)/(Ra*Kb) ,  K*Kt»Kp/(Ra*Ti)  3 

ezternal.lnput  u*l 
external. output  z 

•  Set  up  the  plant 
connection  plant.lnput  ■  n 
connection  z  *  plant. output 

HNNHNmimtnNNHNftHHNIffUHIHttMmitIHNnHH 

"  Input  File  for  SIMNON 

MHHIIniinHMIlHH  lilt  milt  till  IIH«f  mill  nHHMHMHIl 

continuous  system  CSPEED 


73 


N  Contlnuoua-Tima  llnaar  ayatam  for  tha  motor  apaad  control 
Input  a 
output  j 

atata  z 

dar  dz 

dx— Kt/ < J*Ra*Kb) *x*K*Kt/ ( J*Ra) *u 
y-z 

yout»z 

K:6 

Kt: 21.62 
Kb : 2 . 443 
J:0.1 
Ra:SS.6 
END 

dlacrata  ayatam  DSPEED 

"Dlacrata-Tima  PI  Faadback  for  tha  motor  apaad  control 

Input  yr  y 

output  u 

atata  1  ] 

naw  nl  aj 

tlma  t 

taamp  ta 

u«l 

nl«l-Kp*  Cy- J ) ♦ (Kp*h/Ti) * (yr- J ) 

nj-y 

ta*t*h 

Kp: 1.07312 

Ti:2.O3340a-l 

h:0.002 

END 

continuous  ayatam  CSPEED 

"Contlnuoua-Tima  llnaar  ayatam  for  tha  motor  apaad  control 
Input  u 
output  y 

atata  z 

dar  dz 

dz— Kt/ ( J*Ra*Kb} *x*K*Kt/ ( J*Ra)  *u 

T"x 

yout*x 

K:6 

Kt  :21 .62 
Kb : 2 . 443 
J:0.1 
Ra:33.6 
END 
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Appendix  C 


Input  File  to  CONSOLE  and  SIMNON 
for  Integral  Control  of  the  Flexible  Arm 


. . . . . . 

Input  Ill*  for  CONSOLE:  specification  description  fils 

*********************************************** ***********/ 
PI  -  3 . 1416026 
HI  «  1/ (0*2*PI) 

R2  »  1/(20. 6*2*PI) 

R3  «  1/ (8.5*2*PI) 

design. parameter  ki  init“8 . 23015  vari“1.43e-l 
design.parameter  k2  init*-2 .78S27e-01  vari»0.0e-3 
deslgn.parameter  k3  inlt--2 . 23363*-04  vari*8.4e-6 
design.parameter  k4  init--3 . 564**6  vari*1.7*-6 
deslgn.parameter  kB  init— 5.7878*1  vari«4.S4el 
design.parameter  k6  init«5 .78548e-l  vari«0.5e-l 
design.parameter  k7  init«0.02761a-l  vari»l*-2 
design.parameter  ■  lnlt>l .03611*2  vari“l*l 
design.parameter  p  init»2 . 38868el  vari“l*0  min«0 . 1 
functlonal.obj active  "over-short" 
for  t  from  0  to  2  by  .0005 
minimize  { 

double  YtrO  ; 
return  Ytr(l,t)/0. 15664;} 
good.curve  ■  { 

lf(  t  <•  0.8  )  return  1.06; 
else  return  1.005;  } 

bad.curve  ■  { 

lf(  t  <■  0.8  )  return  1.1; 
else  return  1.01;  } 

functlonal.obj active  "settling" 
for  t  from  0.5  to  2  by  .0005 
maximize  { 

double  Ytr() ; 

return  Ytr(l,t)/0. 15664;  } 
good.curve  ■  { 

if (  t  <■  0.6  )  return  0.0; 
else  return  0.080;  } 

bad.curve  ■  { 

lf(  t  <■  0.6  )  return  0.85; 
else  return  0.00;  } 

objective  "steadystate" 
minimize  { 

double  Ytr() ; 

return  fabs(  Ytr(l, 100.0) -0.15664  };  } 
good.value  ■  0 
bad.value  *  1 

functional .constraint  "max-accol"  hard 
for  t  from  0  to  2  by  .005 
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<  double  YtrO: 
return  Ytr(2,t) ;  > 

<■  good.curve  ■  <  return  10;  > 
bad.curve  *  {  return  11;  > 

functlonal.conetralnt  "max-accel"  hard 
for  t  froa  0  to  2  by  .005 
{  double  YtrO; 

return  YtrO.t);  > 

>■  good.curve  ■  {  return  -10;  > 
bad.curve  *  {  return  -11;  > 

functlonal.conetrelnt  "control**  hard 
for  t  from  0  to  2  by  .002 
i  Import  kl  k2  k3  k4  kE  kO  k7 
double  YtrO ; 

return  fabeC  l-(kl*Ytr(3.t)*k2*Ytr(4.t)*k3*Ytr(6.t)*k4*Ytr(8,t) 
*k6*Ytr (7 . t) *k6*Ytr (8 . t) *k7*Ytr (0 . t) ) ) ;  > 

<■  good.curve  ■  {  return  6;  > 

bad.curve  ■  {  return  0;  > 

constraint  "observl"  hard 
{  Import  a  p  Rl 

return  faba(  (-p-m)*(-p-m)*Rl*Ri*0.22*Ri*(-p-m)*l  ); 

>  >■  good. value* 1 

bad.value  *0.01 

constraint  "oboerv2"  hard 
{  Import  ■  p  R2 

return  fabs(  (-p-n)*(-p-m)*R2*R2*0.04*R2*(-p-m)*l  ); 

>  >*»  good,  value"  1 

bad.value  "0.01 

constraint  "observS"  hard 

<  Import  m  p  R3 

return  fabs(  13*(-p-m)*l  ); 

>  >■  good_value*l 

bad.value  "0.01 

constraint  "controll"  hard 

<  Import  p  R3 

return  fabs(  -p*p*p*R3*R3*2.1*R3*p*p-p  ); 

>  »•  good.value-l 

bad.value  *0.01 

Input  file  for  CONSOLE:  system  description  file  #*•## 

K-0.02 

Ib-2.443 

PI  •  3.1416020 

0  ■  0.125 

Kl  .  0*2*PI 

R2  -  20.6*2*PI 

R3  -  0.5*2*PI 

14  -  Kb*G/ (0.383*10 

A1  ■  0.04*12*0.22*11 

A2  -  11*11*0.0088*12*11*12*12 

A3  -  0 . 22*R2*R2*R1*0 . 04*R2*R1*11 

A4  *  !2*R2*R1*!1 

B2  -  R1*R2*R1*R2/ (R3*R3) 

B3  -  2*B2*R3 
B4  ■  B2*R3*R3 
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Cl  ■  (A1*A1-A2) *0.383*84 
C2  -  -Ai*0.383*R4 
C3  -  0.383*R4 
C4  -  B2*0.383*R4 
b«  -  2*PI*1 . 16*K/Kb 


ayatam.aixa  Ninputa»l  Natataa>7  Noutputa“9 


raadmatrlx  A 

-A1 

1 

0 

0 

0 

0 

-A2  0 

1  0 

B2 

0 

0 

-AS 

0 

0 

1 

B3 

0 

-A4 

0 

0 

0 

B4 

0 

0 

0 

0 

0 

0 

1 

-b6*ki 

-b6*k2 

-b6*k3 

-b6*k4 

-b6*k5 

-7.2B-b6*k6 

m*R4 

0 

0 

0 

0 

0 

0 


0 

0 

0 

b6*k7 

-P 


raadmatrlx  B 
0 
0 
0 
0 
0 
be 
o 


raadmatrlx  C 
R4  0  0  0 

Cl  C2  C3  0 

10  0  0 
0  10  0 

0  0  10 

0  0  0  1 

0  0  0  0 

0  0  0  0 

0  0  0  0 

raadmatrlx  Ut 
1 


0  0  0 

C4  0  0 

0  0  0 

0  0  0 

0  0  0 

0  0  0 

10  0 
0  10 

0  0  1 


NHtllKIHtlHNHmiNItHHNIMtHNIMHIHIIIIItlMINHHfltMinNHIMMIN MMMMftftfMt  M  IIIIHNH  II 

"  Input  tlla  for  SIMNON:  Plant 

MMHnillin  ««<•«'<  II  IIHnMIin'tHHMIltl  IMIHIIIHtMtlUmiltllHIlHMIMIIIMIMIItllHIlHHIIMIIIt 

contlnuoua  ayatam  CFXXNT 

"Contlnuoua-Tima  linaar  ayatam  for  tha  flaxlbla  arm 
Input  u 

output  yl  y2  y3 

atata  xl  x2  x3  x4  x6  x6 

DER  dxl  dx2  dx3  dr 4  dxB  dx6 

dxl>all*xl*al2*x2 

dx2"a21*xl+a23*x3+a2S*x6 

dx3«a31*xl*a34*x4+a35*x6 

dx4*a41*xl+a45*x5 

dx5«a56*x6 

dx6=a66*x6'*’be*B 

y*cl*xl 

yl»x5 

y2*x6 

y3«c31*xl*c32*r2+c33*x3*c35*x6 

all : *17 . 6180513008 

al2:l 

a21 : *20015 . 22226558215 
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•23:1 

•26 : 16036 . 08801734764 
•31 : -224076 . 7773142842 
•34:1 

•36 : 1706004.060181533 
•41 : -63572128 . 68060316 
•46 : 63572128.68060316 
•66:1 
•66: -7. 20 

b6: 17. 06018783816610 

el :0. 16365866 

c31 : -000 . 6616048178267 

c32 : -0 . 8037063813023266 

c33:0. 06072674418604661 

c35 : 762 . 7267177404406 

and 


""""""  Input  File  for  SIMNON:  Controller 
diacrata  ayatem  DFXAAN 

"Diacrete-Time  controllar-obaarvar  for  tha  flaxlbla  arm 
INPUT  u 

OUTPUT  ▼  yl  y2  y3 
atata  xl  x2  x3  x4  x6  x6 
new  nxl  nx2  nx3  nx4  nx6  nx6 
time  t 
taamp  ta 

nxl»all*xl+al2*x2+al3*x3+al4*x4+al6*x5*al6*x6+bl*v 

nx2“a21*xl*a22*x2+a23*x3+a24*x4*a26*xE+a26*x6+b2*w 

nx3»a31*xl+a32*x2+»33*x3+»34*x4+a3B*xB+»36*x6+b3*y 

nx4*a41*xl+a42*x2+a43*x3+a44*x4+a4E*xE«a46*x6+b4*v 

nx5“a66*x6*a66*x6»b6*T 

nx6-a66*x6+b6*v 

T«niref-kl*xi-k2*x2-k3*x3-k4*x4-k6«x6-k6‘x6  "Feedback  law 

yl«xE 

y2«x6 

y3Bc31*xl+c32*x2+c33*x3+c35*xE 

ta-t+h 

•11:0. 0423342040036036 
•12:0. 006288706726673081 
•13:3. 0683 18 102470788a-06 
•14:1 .44 10247 66083601a-07 
•16:0. 8766638674786606 
•16:0.002833861583283286 
•21: -142. 624116824618 
•22:0. 1631306474427612 
•23:0. 006087037061601664 
•24:4. 222367236716648a-06 
•26:188.0410331260212 
•26:0. 0040320220006725 
•31 : -3640 . 730460810261 
•32 : - 16 . 6624626601023 
•33 : 0.0473082638713216 
•34:0.000873081600827604 
•36:10608.46077632080 
•36 : 04 . 06500866750708 
•41 : -336004 . 2275302042 
•42 : -2126 .012628886066 
•43 : -7 . 724607860376001 
•44:0. 0708380681641863 
•46 : 381166 . 0406303600 
•46:2238.06116014823 
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*66:1 

*66:0. 009644108260074846 
*66:0. 9296937947569644 
bl:0. 0001086674552153263 
b2 : 0 . 04678293172620013 
b3:4. 821345333692120 
b4: 124.2241266603102 
b6:0.00076SE668825641S74 
b6 : 0 . 1402995476678602 
c31: -1169. 653162308412 
e32:-l. 036843886404806 
c33:0. 06886122410646139 
c36 : 884 . 8863004569524 
kl:30. 36213161279467 
k2: -0.1644232615164464 
k3: -0.001473703749071624 
k4 : -6 . 848483626764808*-06 
k6 : -27 . 60063350991406 
k6:0. 6208171671299313 
h:0.01 
oral :1 
END 

"""  Connecting  File:  Interface  between  Controller  end  Plant  """ 

CONNECTING  SYSTEM  fxintcon 

yldif «yl [cf  xlnt] -yl [df xlnt] 

y2dif *y2 [cf xlnt] -y2 [df xlnt] 

y3dlf *y3 [cf xlnt] -y3 [df xlnt] 

u[df xint]=v [df xlnt] ♦ll*yldif ♦12*y2dif ♦13*y3dif 
n [cf xlnt] «v [df xlnt] 

11:10.16 
12:. 76 
13:0.06 
END 
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Appendix  D 


Microsoft  C  Program  Listing  of  the  Integra!  Controller-Observer 

for  the  Flexible  Arm 


ilncluda  <proeeaa.h> 
ilncluda  <conlo.h> 
tlneluda  <atdlib.h> 
ilncluda  <atdio.h> 
ilncluda  <nath.h> 
axtarn  lnt  daahlOO ; 
axtarn  lnt  dda6(); 
axtarn  void  axlt(); 
axtarn  lnt  aba(); 
axtarn  FILE  *fopan(); 
axtarn  lnt  fprintf  0 ; 

lnt  node,  baaadl6*784,  dl61o[6],  flag; 

lnt  baaeddafi*768,  pa,  pb,  ddafiof at*2006; 

lnt  zero*0,  one*l,  three-3,  eleven-11,  four* 4 .  tvelva-12; 

nalnO 

< 

FILE  afpl,  *fp2,  *fp3,  *fp4,  *fp6,  *fp«; 
char  acomma  ■ 

lnt  not.inp,  nodaao,  noloop; 
lnt  1,  J ; 

lnt  loopcount,  loopofat; 

doubla  poa.raf ,  vel,  poa,  accal,  ou.foree; 

doubla  uf ,  cur.force,  accaldlf ,  poadlf,  veldlf,  accalat; 

doubla  valofat,  accalofat,  posofst; 

doubla  atl,  at 2,  at3,  st4,  atfi,  atfl; 

float  poaplot [600] .  velplot[800] ,  accalplot  [800] ,  atata6[800]; 
float  atata6[800],  atataaccal  [800] ; 

doubla  naxtatl,  naxtat2,  naxtatS,  naxtat4,  naxtatS,  next atfi; 
doubla  z[1000],  a,  h,  p,  zy,  axphp; 

doubla  contfdbk*0,  u_forca,  u.tmp ,  a urn,  accalgaln,  velatgaln; 
doubla  offaat (),  rlptorq,  rlpgaln,  ripofat,  tima; 
doubla  Tf,  vpoa,  vnag,  vof,  alphal,  alpha2,  gammal,  gammal; 
doubla  n_lov,  n.hlgh; 


/••••*  Initialization  of  daahlO  aa***/ 
lnltdlSO ; 

/aaaaa  Open  Fllaa  aaaaa/ 

if(  (fpl  -  fopen(  "itglpoe.plo",  "w"  ))  ■■  NULL  )  < 
fprintf (  atdarr,  "couldn’t  opan  flla  ltgl2.plo\n"  ); 
axlt(  1  ); 

} 


/aaaaa  Conpute  tba  offaat  value  for  velocity  and  acceleration  aaaaa/ 
valofat  ■  offaat (  0,  2.426  ); 
accalofat  ■  offaat (  4,  1.0  ); 

print! ("tach  offaat:  Ilf  accal  offaat:  Xlf\n",  valofat .accalofat) ; 
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/eeeee  Start  Elocution  ****0/ 
print!  ("  Start  axocutlon\nM) ; 

■ot.lnp  *  ddaOofst; 
ddaflC  mot.lnp  ): 

tlno  ■  0.003; 
rlpgaln  *0.10  *  204.8; 
ripofet  ■  13.6; 

▼pos  "  245; 

imeg  ■  -220;  /•  1.31  •  204.8  •/ 

wo!  ■  0.03  •  204.8; 

alphal  ■  -0.1; 

gammal  *  0.02; 

alpha2  ■  -0.06; 

gamma2  *  0.021; 

print! ("  Input  atop  slza  (  5  or  loaa)  ") ; 

acan! ("Xlf",  Apos.ref  ); 

i!(  !aba(  poa.ro!  )  >  6  )  poe_ref«E; 

print! C"  How  many  demos  ") ; 

ecan!("Xd",  frnodemo  ); 

print! (  "No  0!  Demos  :  Xd\n",  nodemo  ); 

u.hlgh  -  2000; 
n.low  ■  -2000; 
cur_!orcs  ■  0; 
outp(  basedda6«15,  Ox bo  ); 

■  »  1.20263ol; 
p  -  1.07204sl; 
h  ■  0.01; 


exphp  ■  sxp(  -h*p  ); 
zy  ■  ■  *  (1  -  exphp)  /  p; 
noloop  ■  600; 
loopeount  ■  1000; 
loopolst  ■  18; 

/eeeee  Initialization  of  the  loop  ••*••/ 
stl  ■  st2  *  st3  ■  st4  ■  stE  ■  st6  ■  0; 

/eeeee  Start  the  loop  eeeee/ 


!or(  1  ■  0;  1  <  nodemo;  lee  )  { 

x[0]  •  0; 

for(  J  ■  0;  J  <  noloop;  Jee  )  < 

/•  Load  Counter  0  ■  loopeount  •/ 
node  ■  eleven; 
dl61o[0]  »  loopeount; 
daehl0(  Anode,  dl01o  ,  if  lag  ); 

1!  (  flag  >  0  )  error (  flag,  node  ); 


/eeeee  26  Hz  Filter  eeeee/  \,' 

cur. force  ■  0.061  •  pos.ra!  e  0.30  •  cur .force; 

/eeeee  Input  velocity  eeeee/ 


■ode  ■  one; 

dl61o[l]  ■  dl61o[0]  ■  sere; 
daahl6(  inode,  dlOlo,  if lag  ); 
lf(  flag  >  0  )  error (  flag,  soda  ); 

■ode  «  three; 

daahl6(  inode,  dl61o.  if lag  ); 
lf(  flag  >  0  )  error (  flag,  node  ); 

▼el  *  dl61o[0]  e  2.426  -  velofet;  /*  2.426/204.8  •/ 
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/*♦*•*  Inpat  accalaratlon  •****/ 

■ode  ■  one; 

dieioCl]  ■  dl6io[0]  ■  four; 
deehl6(  tonode ,  dl61o,  dfleg  ); 
lf(  flag  >  0  )  arror(  flag,  noda  ); 

■ode  ■  thraa; 

deshl6(  fcmode ,  dl61o,  Jkflag  ); 
lf(  flag  >  0  )  error (  flag,  soda  ); 
accal  »  dl6io[0]  -  accalofat;  /*  1/304.8  •/ 

/****•  Input  poaltlon  data  aa*a*/ 
pa  *  inp(  basedda6+12  ); 
pb  >  lnp(  baaedda6+13  ); 

poa  -  C (pb*2E6+pa)  »  4)*0. 3141602663 ;  /*PI/10*/ 


/****•  Compute  tha  rlppla  torqua  a****/ 

riptorq«ripg*in*fin(0.2002*(poa-ripof at+tlme*vel)) ; 
/*  0.2002  ■  41/204.8  •/ 

/****•  Computing  tha  feedback  value  ***•*/ 


contfdbk  -  1.830fi7646al  *  atl  -  1.088404649-1  *  at2 

-2.288609-3  *  at3  ♦  1.471S0Ea-B  *  at4 
♦1.0068746E6el  *  atB  ♦  7.3760066a-l  *  atB 
-2.30013a-7  *  a[J] ; 
u_tmp  ■  eur.forca  *  204.8  -  contfdbk; 

/***  computa  tha  friction  compensation  feedback  •  **/ 
if  (  vel  >  wpoe  )  Tf  ■  gammal*vel*30; 

alee  if(  wpoa  >-vel  ft&vel  >  wof  )  Tf  ■  alphal*vel+B0; 
alaa  if(  wof  >-vel  ktvel  >■  -wof  )  { 

lf(  u.tmp  >  2.8  )  Tf  •  50; 

alaa  lf(  u_tmp  <  -2.5  )  Tf  »  -40; 
alaa  Tf  ■  0; 

> 

alaa  if (  -wof  >vel  ftftvol  >■  wnag  )  Tf  ■  alpha2*vel-40 ; 
alaa  Tf  -  gamma2*vel-20; 

/*  BO  b  0.288*204.8  40.14  -  0.106*204.8  */ 

u.forca  «  u.tmp  -  riptorq  ♦  Tf; 
uf  ■  -u_f orce ; 
if(  uf  >  2040  )  uf  «  2040; 

lf(  uf  <  -2040  )  uf  «  -2040; 
aot.inp  ■  uf  ♦  2005; 
ddafi (  mot.inp  ) ; 

poadif  >  poa  *  atB; 

poaplot[j]  ■  poa; 
yaldlf  ■  -wal  ♦  at6; 

accalat  «  0.22  *  C-9.00B61605e2*atl-0.8037063814*at2 

♦  5.0726744186a-2*at3  ♦  7.6272671774e2  •  atB); 
accaldif  «  -accal  -  accalat; 

/*  add  tha  aaturator  to  tha  Input  of  obaaryer  •/ 
lf(  u.forca  <  n_low  )  u.forca  ■  u.low; 

alaa  lf(  u.forca  >  u.hlgh  )  u.forca  ■  u.hlgh; 

alaa 

ou_forca  ■  u.forca  -  (10.15  •  poadif  ♦  0.76  *  yaldlf 

*  0.06  *  accaldif) ; 

nartatl  -  4.2334203834a-2  *  atl  *  6.288706731a-3  *  at2 

♦  3.068318103883a-6  *  at3  ♦  1 .44102476336e-7  •  at4 

*  8 .75663867 161a- 1  *  atB  ♦  2.833851B78673a-3  •  at6 
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♦  1 . 2E9658*-4  *  ou.forc*; 

n*xtat2  »  ~1. 426241169*2  «  *tl  *  1.6313064T262*-1  *  *t2 

♦  6 .987937066*-3  *  atS  ♦  4 . 22236723666*-6  •  *t4 

♦  1.889410332*2  •  *t6  ♦  0 .049320216434*-1  *  *t6 

♦  6.31166024*-2  •  ou.forc*; 

naxtatS  -  -3.6407394626*3  •  *tl  -  1.666246266*1  •  *t2 

♦  9.47308264*-l  •  *tS  ♦  9.87398151*-3  *  *t4 

♦  1.969846077627*4  *  at6  *  0.405509866862*1  *  ate 

♦  6.6936400404  *  ou.forc*; 

n*xtat4  -  -3.3690422778*6  *  atl  -  2.12691263*3  *  at2 
-  7.724607846  *  at3  ♦  0.708380682*-!  *  et4 

♦  8.8116604088463*6  •  at6  ♦  2.238081140*3  *  at6 

♦  1.4412007*2  *  ou.forc*; 

naxtatS  -  et5  ♦  O.64419826*-3*at6*8.7667073413*-4  *  on.forc*; 

n*xtat6  ■  9. 20693794767*- 1  •  *t6  +  1.7321161212*-1  *  on.forc*; 
*[j+l]  *  *xphp  *  ztj]  ♦  xy  *  atl  *  0.1324468; 

atl  *  naxtatl; 

»t2  -  n*xtat2; 
at3  “  naxtatS; 
at4  “  n*xtat4; 
at6  ■  naxtatS; 
at6  “  n*xt*t6; 

mod*  *  twelve; 

dl61o [0]  *  on*;  /*  Latch  bafor*  raad  */ 
dashl6(  tmod*.  dl61o,  hflag  ); 

If  (  flag  >  0  )  *rror(  flag,  mod*  ); 
while (  dl61o [1]  >  loopofat  )  { 

/*  Reading  Counter  0  */ 
mod*  •  twelve; 

dieiotO]  ■  on*;  /•  Latch  bafor*  road  */ 
da*hl6(  ftmod* .  dl61o,  ftflag  ); 
if  (  flag  >  0  )  error (  flag,  mod*  ) ; 

> 

poa_r«f  •  -poa.ref; 

> 

'•••  Th*  End  •••••/ 
dda6C  2006  ); 
printfC'End  of  loop\n") ; 
for(  J  ■  0;  J  <  noloop;  ) 

fprlntf(  fpl,"XdX»Xlf\n".  J.  comma,  po*plot[j]); 
prlntfC'Th*  End*.\nH); 


/ 


> 
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